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TABLES OF SYMBOLS AND ABBREVIATIONS 



Text 

Variable 


Computer 

Symbol^ 


Description 


a 


SMALLA(i) 


shortest distance along common normal 
between joint axes 


a 


n/a 


end-effector approach vector 


A 


A(-.-.i) 


kinematic link coordinate matrix 


b 


B(i) 


bias vector 


c 


CC(-,i) 


recursive constant 


c 


n/a 


matrix for centrifugal/ Coriolis effects 


d 


SMALLD(i) 


distance between adjacent links 


D 


DD(-,-,i) 


recursive constant 


e . 
J 


EJ(i) 


link unity vector 


g 


GRAV(-,i) 


gravity vector 


G 


n/ a 


vector of effects due to gravity. 


h 


n/ a 


height 


h. 

J 


HJ(i) 


link inertia vector 


H 


n/ a 


program inertia matrix. 


I 


ACTIA(i) 


actuator inertia 


I 


n/a 


identity matrix 


J 


EETIA(-,-,i) 


link inertia matrix 


k 


FMK(i) 


external moments/forces vector 



n/ a denotes ''not applicable'' 
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K 


n/ a 


Jacobian matrix specifying forces 
created at each joint due to external 
forces and moments 


k* 

XX 


B2XX(i) 


radius of gyration about the xx axis 


k* 

yy 


B2YY(i) 


radius of gyration about the yy axis 


k» 

zz 


B2ZZ(i) 


radius of gyration about the zz axis 


^actuator 


ACTIA(i) 


actuator kinetic energy 


KE 


n/a 


Kinetic energy 


n/ a 


KNOTS 


total Number of knots to be processed 


L 


n/ a 


Lagrangian 


H 


n/ a 


vector of forces on each joint actuator 


n 


LINKS 


maximum number of manipulator links 


n 


n/ a 


end-effector normal vector 


o 


n/ a 


end-effector orientation vector 


P 


n/ a 


end-effector position vector 


r 


n/ a 


general position vector 


PE • 


n/a 


potential energy 


4 


n/ a 


generalized joint displacement 


# 

4 


n/ a 


generalized joint velocity 


• • 
4 


n/ a 


generalized joint acceleration 


t 


TIME 

TM(i) 


time 


At 


STEPSZ 


time difference between knots 


T 


Tr(-.-.i) 


kinematic transformation matrix 


• 

T 


TDl(-,-,i) 


kinematic transformation velocity matrix 


• • 
T 


TD2(-,-,i) 


kinematic transformation acceleration 



matrix 
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X 


XBAR(i) 


y 


YBAR(i) 


z 


ZBAR(i) 


a 


ALPHA(i) 


0 


T(i) 

THETA(i,knot) 


• 

e 


TD(i) 

THETD(i,knot) 


« • 

e 


TDD(i) 

THTD2(i,knot) 


• 

e 

a 


THETD(i,knot) 


e 

a 


THETA(i,knot) 


• 

e. 

1 


TLAST(i) 


e 

m 


n/ a 


e 

m 


n/ a 


• • 

e 


ACLSIM(i,knot) 


S 


ACCS IM(i, knot) 
TDDSIM(i) 


• 

0 


VELSIM(i.knot) 


s 


TDSIM(i) 


e 

s 


POSSIM(i.knot) 

TSIM(i) 


X 


TAU(i) 

TORQUE(i,knot) 


3A. 




1 


PA(-,-,i) 


ae. 





1 



first moment of x axis 
first moment of y axis 
first moment of z axis 

twist# angle between joint axes in plane 
perpendicular to a^ 

joint angular position# angle between 
adjacent links. 

joint angular velocity 

joint angular acceleration 

actual joint velocity storage matrix 
actual joint angle storage matrix 

finite difference velocity 

modelled joint velocity 
modelled joint velocity 

simulated joint angle acceleration 

simulated joint angle velocity 
simulated joint angle position 
joint torque 

first partial derivative of matrix A 
with respect to 0 
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a*A. 






1 


PPA(-,-,i) 


second partial derivative of matrix A 


ae^» 




with respect to 6 


3T 






1 


PTQ(-,-,i) 


first partial derivative of matrix T 


ae, 




with respect to 0 
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I. INTRODUCTION 



A. MOTIVATION FOR SIMULATION 

The U.S. Navy is developing robots to help meet the Navy's 
automation needs. According to VADM Fowler [Ref. 1], ''The 
construction of the 600-ship (US) Navy requires ever-improving 
productivity through the adoption of the most cost-efficient 
manufacturing technology.'' A robot, as defined by the Robot 
Institute of America (RIA) [Ref. 2], is: ''a reprogrammable 

multifunctional manipulator designed to move material, parts, tools, 
or specialized devices, through variable programmed motions for the 
performance of a variety of tasks.'' Robot manipulator simulation 
allows the determination of arm dynamic responses to various 
permutations of input data by mathematically modelling the proposed 
configurations. 

B. MODELLING AND SIMULATION IN DESIGN 

Simulation software can model diverse components of a robot, even 
within the context of an entire robotic manufacturing cell which is 
comprised of the robot manipulator, end effector, and workspace 
elements. Engineers can use this software to predict dynamic 

performance long before expensive mechanisms have been constructed. 
Physical parameters and design configurations can be selected and 
optimized, and complicated control schemes can be modelled and 
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evaluated. Additionally, engineers can focus their research 
attention on the inherent nonlinear kinematic and dynamic physical 
relationships of a robot manipulator arm to its workcell. 

1 • Performance Prediction 

The dynamic response predicted by a manipulator arm simulator 
can be of inestimable value to robotic engineers, designers, and 
users in many areas involving the effects of physical design values 
on performance. For instance, accurate modelling and simulation can 
be used in: 

1. manipulator arm design 

2. robotics educational training 

3. manipulator arm selection 

4. performance evaluation 

5. trajectory planning 

6. trajectory evaluation 

7. workspace layout 

8. control programming 

9. singularity point determination 

10. cycle time determination 

11. dynamic property calculation 

12. actuator/link loads and size determination 

13. error effect evaluation (i.e. backlash) 

14. actuator overload predictions 

15. parameter optimization 

2. Design Parameter Selection 

Manipulator dynamic analysis must aid in the mechanical and 
structural design of prototype arms. Modelling and simulation can 
hasten robot arm development by permitting an engineer to examine 
kinematic and dynamic behavior and better understand the significance 
of various parameters before constructing a manipulator arm. The 
forces and torques predicted for a mechanical arm during movement can 
yield data useful in designing actuators, joints, and structures by 
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making the role of primary design parameters explicit. 



Discrete 



dynamic coefficients can be numerically evaluated to determine their 
relative magnitude and importance. Additionally, a valid model can 
be used to identify negligible dynamics and the corresponding 
simplified design and programming problem. 

3 • Control Schemes 

Robot arm control maintains the dynamic response of a 
computer-based manipulator in accordance with some predetermined 
system performance goal. With a valid model, simulation furnishes a 
testing technique for control strategies without the expense and 
mechanical problems of working with actual manipulators. These 
techniques can also include the off-line programming and testing of 
suitable optimal and sub-optimal control schemes. Control laws can 
be evaluated numerically so that dynamic effects can be resolved 
without inflicting damage* or tying up an actual manipulator system. 

4. Robot Teaching and Workspace Layout 

Robot arm simulation allows development of robot planning and 
programming and can provide significant time savings through robot 
physical modelling and workspace layout. 

C. THE ROBOT MODELLING AND SIMULATION PROBLEM 

The robot arm dynamic modelling problem is one which requires the 
formulation of an accurate and efficient mathematical representation 
of all components involved in manipulator dynamics. Effective 
mathematical formulations are used to predict the amount of actuator 



17 



torque required to produce a specific motion or they are used to 
predict the motion given the input torque. In this work the torque 
on the arm joints was assumed as a known function of time. 



® 

i a 



Trajectory 




Control 




Dynamic 




Integrators 


Design 




System 


r 
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Model 


• • 
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m 



m 



Figure 1, Manipulator Model Block Diagram 



A reasonably complete version of an entire manipulator model 
includes a control system, as shown in Figure 1, Here the actual arm 
dynamics are replaced by a mathematical dynamic model of the 

manipulator. Model accuracy is determined by comparing modelled 

• . • 

trajectory information (6 ,6 ) against actual velocities (6 ) and 

mm a 

positions 

This thesis does not consider the mathematical formulation of 
trajectory design, control, or feedback systems, as shown in Figure 
1, It does combine a previously developed mathematical formulation 
with a method of modelling robot arm movements developed by Walker 
and Orin [Ref, 3], A simplified robot arm simulation problem can be 
stated as: given a manipulator dynamic model, it's link mass and 

inertia properties, it's initial link alignments, and the joint 
force/ torque signal and limits, then predict the motion of each joint 
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and, ultimately, the end-effector (i,e* individual displacements, 

velocities, and accelerations versus time). The simulation is not 

updated with actual velocity and position data, as is the model of 

Figure 1, but runs on its own predictions, as shown in Figure 2, 

Simulation accuracy is evaluated by comparing predicted values (6^) 

to actual values (6 ). 

a 



^ V 


Dynamic 








Model 


. . 


Integrator 






e 








s 








e 

S 



Figure 2. Manipulator Simulation Block Diagram 
1. Current Solution Methods 

Many methods of accomplishing computer simulation of 
complicated manipulator mechanisms have been developed recently. An 
examination of the more prominent simulation techniques was made and 
a summary of these follows. 

Vereshchagin developed a method which uses a direct 
implementation of Gauss's principle of least constraint [Ref. 4]. 
Nelson and Chang explored nonlinear motor current and voltage limit 
effects when simulating a specific robot arm having three Cartesian 
axes of motion with two rotational wrist axes [Ref. 5]. 
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Murray/Neuman and Cesareo/Nicolo have developed a computer 
program which can symbolically generate the dynamic equations of 
motion [Ref, 6,7] • 

Meyer/Jayaraman, Leu/Mahajan, Wang/Lin, and Staufer showed 
the practical use of graphics in simulation software [Ref. 
8,9,10,11]. Backhouse described a dynamic simulation which was 
implemented on an analog computer and included the modelling of 
hydraulic motors and servo-valves [Ref. 12]. 

Brandeberry/Dufour/ Spalding have formulated specific dynamic 
equations for a small robotic arm they are designing [Ref. 13]. 
Cvetkovic/Vukobratovic developed dynamic modelling using the Newton- 
Euler formulation with recurrence relations for velocities, 

accelerations, and forces [Ref. 14]. Derby has considered simulation 
using cam-motion profile techniques with the General Robot Arm 
Simulation Program (GRASP) [Ref. 15]. 
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II, BASIC MANIPULATOR CONCEPTS 



A. MANIPULATOR ARM CONFIGURATION 

An understanding of manipulator kinematics, statics, and dynamics 
is fundamental to designing a simulation system. The Robot 
manipulators considered in this thesis are composed of serially 
connected, rigid linked structures, jointed by one-degree of freedom 
joints. Manipulator joints which rotate about an axis are called 
revolute and those which change their position along an axis are 
called prismatic. The principal degree of freedom for revoluate 
joints is the joint angle, and for prismatic joints is the translated 
distance. At the end of a manipulator chain, there is usually some 
tool or hand/ gripper mechanism which performs the actual task for 
which the robot was designed. Due to their variety and complexity, 
the devices placed in this position are designated as end-effectors. 
Joint actuators are typically located between adjacent links to allow 
proper joint positioning. 

B. KINEMATICS 

The basic robotics problem is to perform a task. Motion planning 
is needed to map the composite motions required for each overall 
task. In planning manipulator movement, the primary task is to 

achieve a specified end-effector Cartesian position with respect to 
the robot^s environment. A kinematic evaluation is used to represent 
the positional relationships of the manipulator mechanism. 
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1 • The Direct Kinematics Problem 



In direct kinematics, a joint coordinate system is translated 
into a fixed coordinate system. Fixed or ''world'' coordinate 
systems must be established and the origin can be chosen at any 
location, including the foundation of a conveyor belt, a central 
geographical location, or even the base of the robot. For the 

purposes of this thesis, the robot base will be used as the fixed 

coordinate system origin. Further details concerning this are 
provided in section II.A.3*b which follows. The direct kinematics 
problem is: given the robot arm's joint angle vector, find the 
manipulator's end effector position and orientation with respect to 
the fixed coordinate system. This problem has an analytically unique 
solution. [Ref. 16] 

2. The Inverse Kinematics Problem 

The inverse kinematics problem can be stated as: given the 
position and orientation of the end effector with respect to the 
fixed coordinate system, find the robot arm joint angles. This 

problem does not have a unique solution due to the transcendental 

nature of the kinematic equations. [Ref. 16,17,18] 

3 . Link Representations 

A link coordinate frame (attached to the body) is shown in 
Figure 3. A summary of the link numbering convention and specific 
procedural steps for establishing link axes and defining link 

parameters are provided in Appendix A. 
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Figure 3. Link Coordinate Convention 

An essential part of the kinematics problem is to find a 
transformation matrix that relates the body-attached link coordinate 
frame to the fixed coordinate frame. General solution methods for 
the kinematic problem have been proposed by several researchers [Ref. 
18,19,20,21] and summarized below. 

a. Denavit-Hartenberg Convention. 

Denavit and Hartenberg were the first to relate spatial 
configurations between neighboring robot arm links using a matrix 
representation of rigid link kinematics [Ref. 22,23]. Uicker 
extended these methods to a generalized displacement analysis [Ref. 
22,24]. Under Denavit and Hartenberg' s well developed and popular 
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convention 



the kinematics solution for each link's position and 



orientation is founded upon a 4x4 homogenous link transformation 
matrix. The individual Denavit-Hartenberg link coordinate matrix, or 
A matrix, is a 4x4 transformation matrix from link i's coordinate 
system to link (i“l)'$ coordinate system as shown in Figure 3. The 
matrix describes the first link's position and orientation 
relative to the robot base* A^ describes the second link's position 
and orientation relative to the first link, etc. The A matrix is 
expressed as [Ref. 22] : 



cos 


e. 

1 


1 -sin 0. cos a. 
1 1 1 


1 sin 6. sin a. 
1 ^ ^ 


1 a^ cos 


1 

1 




sin 


e. 

1 


1 cos 6. cos a. 
1 ^ ^ 


1 -cos 0^ sin 


1 a. sin 


e. 

1 


(1) 


0 




1 sin a« 

j 1 


1 cos a. 

, 1 


1 d. 

, 1 






0 




1 0 


1 0 


1 1 







and A^ = I, the identity matrix. 

The coordinate system for the manipulator used in this 
thesis, the PUMA arm, is shown in Figure 4. 
b. Transformation Matrices. 

Once an orientation for each link is established by it's 
link transformation matrix, than any link position can be established 
in fixed coordinates. For example, transformation matrices are used 
to map the link 6 (end-effector) position and orientation vectors 
into the robots base coordinate system, as shown in Figure 5. In 
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V 




Figure 4. PUMA Arm Link Coordinate System 
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general, the transformation matrix, T^, is obtained by chain 

multiplying successive link coordinate transformation matrices, 

or: . i 

= n A. ; for i=l,2..,n 

° j=i ^ 

For example: = Tg = 




Figure 5, Position and Orientation Vector of End-effector 



The transformation matrix is comprised of four submatrices as follows 



[Ref. 19,20] 



o = 



n o a I p ' " Orientation Vectors I Position Vector 

® ® ® I ^ J L Perspective Transform I Scaling Factor 



i^rnoapl ^ 

^o-loooij - 



n o a p 

X X X ^x 

n o a p 
O" O" 0^ l" 



(4) 



where : 



n = end-effector normal vector. (Orthogonal to robot arm fingers, 
with a parallel jaw hand.) 

o = end-effector orientation vector. Points in finger motion 
direction. 

a = end-effector approach vector. Points in direction normal to 
hand base. 

p = end-effector position vector. Points from robot base 
coordinate system to origin of hand coordinate system. 

c. Position vectors. 

T 

A position vector r can be represented as (r ,r ,r ,1) . 

X y z 

The '1' allows rotation and translation combinations when applying 
transformations. This is done by multiplying a position vector by a 
link coordinate matrix, as follows: 

Let ^r = vector from link i's origin to a point, 

expressed in the link i coordinate system. 

^ = vector from link (i-l)^s origin to the above point, 

expressed in the link (i-1) coordinate system. 

Therefore ; 

r = ^r. (5) 



i-1 
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Similarly, a vector can be defined from the i 

coordinate system to a fixed point in link j, but expressed in link i 
i T 

coordinates as r. = (r ,r ,r ,1) • Therefore, points in adjacent 

j X y 2 ^ ^ 

coordinate systems are related by: 



1-1 . 1 
r. = A. r, . 
J 1 J 



( 6 ) 



Thus, any positions in two different coordinate systems i and j can 
be related by cascading the transformations: 



•'r, 

k j k 

with as defined in Equation (2). The matrix t! = I, The 
J J 

superscript, 0, is omitted when referring to the base coordinate 

system, thus it can be written r, = and T. = T?. Therefore, 

k jc J J 

*1, = T. •’r, 

k J k 

d. Euler Rotation Transformation Conventions* 



(7) 



( 8 ) 



In addition to the above, robot arm link orientations can 
also be expressed by a sequence of rotations about the x, y, z axes* 
These rotations can be in the form of Euler angles: 



Euler(f,0,¥) = Rotate(z,f), Rotate(y«6), Rotate(z,9) (9) 



9 about the z axis 
0 about the new y axis 
$ about the new z axis 

The Euler homogeneous tranf oimat ion matrix would then be: 



T 



i 



o 



R(i,e,«) I p 1 

1 — 

.0 ® ® I ^ , 



Rotation I Position Vector 

1 

Perspective I Scaling Factor 



( 10 ) 
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C. TRAJECTORIES 



Trajectories consist of the time function of the positions, 
velocities, and accelerations of points on the arm. As previously 
stated, to move a robot manipulator end-effector to a point in 
Cartesian space the individual link joint angles must be found. The 
task of moving the end-effector from one place to another in a timed 
sequence, requires the formulation of a trajectory plan. This can be 
done by converting desired end-effector movements into time- 
serialized movements for all of the manipulator joints. Once 
formulated, each robot arm joint will move through the sequence 
trajectory points. 

The specific values for position, velocity, and orientation 
depend upon the type of trajectory to be followed between workpoints. 
Various methods have been developed to determine trajectory plans 
[Ref, 19,25,26,27], For example, a common class of trajectories 
corresponds to the straight line movement of the end-effector between 
workpoints. Additionally, trajectory plans can depend upon whether 
continuous (smooth) or point-to-point motion is desired. Trajectory 
planning must not produce trajectories which exceed actuator bounds 
or arm stress limits. However, the trajectory planning process is 
seen to be beyond the scope of this thesis. Trajectory values used 
to verify simulation and modelling results were obtained from 
previously published data for the specified manipulator arm. 
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To move a manipulator along a trajectory in accordance with the 
prescribed plan, torques must be exerted by joint actuators. The 
forces/ torques required must be computed. The relationship between 
kinematic states and driving torques is seen during kinetics 
computations, 

D, KINETICS 

System dynamics, which determine the effects of torque on the 
manipulator, are complex due to multiple degrees-of-f reedom and 
complicated interlink relationships. Of principal concern in this 
thesis are those internal forces and moments generated as a result of 
torque application and subsequent motion. Forces generated by 
payload and obstacles are not modelled, 

1, Statics 

Static forces exerted by the manipulator on the environment 
and static link forces should be evaluated in robot modelling. An 
equilibrium analysis of the manipulator system at rest is performed 
by equating all external forces acting on the robot to zero and 
computing the corresponding internal forces, A static force 
evaluation was not done for this thesis, 

2, D* Alembert * s Principle 

Significant contributions to theoretical dynamics were made 
during the eighteenth century by J,R, d'Alembert (1717-83), L, Euler 
(1701-83), and J,L, Lagrange (1736-1813), D'Alembert's principle 
describes a viewpoint where a body's acceleration generates an 
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inertia force which is considered with the other forces acting on the 
body [Ref, 28], This concept of dynamics, with an inertia force 
formulation, creates an artificial state of ^'dynamic'' equilibrium. 
The external forces acting on the manipulator are summed and equated 
to ' 'd'Alembert • ' forces, 

3 , Dynamic Equations of Motion 

Manipulator dynamic equations relate forces/ torques to joint 
angle positions, velocities, and accelerations by taking into account 
link masses and geometic properties. These equations can be 
formulated and solved using one of several approaches. For example, 
one or two link mechanisms can be formulated directly from proper 
free-body diagrams [Ref, 31], With simple mechanisms, the entire 
free-body system can be described in a framework of ''inertial 
coordinates''. However, for more complex systems, Lagrangian, 
virtual work [Ref, 32], or Newton-Euler [Ref, 19,21,33] formulations 
must be used, 

4, The Dynamic Simulation Problem 

The dynamic robot arm modelling and simulation problem is 
restated as follows: Given a manipulator dynamic model, it's link 
mass and inertia properties, it's initial configuration, and joint 
torque signals; predict the joint positions, velocities, and 
accelerations which are be produced at each joint and the end- 
effector, (In the inverse manipulator dynamics problem, which forms 
the heart of control programming efforts, joint torques are 
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determined from the relationships of joint positions, velocities, and 
accelerations. ) 

a. Dynamic Equation Simplification. 

Torque signals must be quickly and accurately computed to 
provide a manipulator with proper control power to carry loads along 
planned paths. In order to accomplish this real-^time control, they 
must be computed on the order of at least 60 times per second [Ref. 
27]. Therefore, there has been strong pressure to increase 
computational efficiency, allowing dynamic equation solution in real- 
time control systems. [Ref. 34,35]. Additionally, as mechanical 
design and control engineers use dynamic analysis to develop better 
manipulators, there will be an increased need to further decrease 
computer simulation time and costs. 

Many analytical schemes have been proposed to make the 
real-time dynamics predictions computationally 'feasible. Most 
significant are dynamic simplifications which ignore some terms and 
correct errors with feedback [Ref. 21]. A good understanding of 
terms and their significance must be developed to allow 
simplification of the dynamic equations of motion in controllers. 

The most common method of simplifying dynamics has been 
to ignore Coriolis and centrifugal forces, which represent the 
greatest computational burden in dynamics calculation [Ref. 21]. 
Other assumptions include the consideration of some system elements 
as massless and the neglect of certain joint offsets [Ref. 19; page 
31]. 
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Results obtained in simplification are typically valid 
only in specific ranges of operation, Coriolis and centrifugal 
forces cannot be ignored at high movement speeds. For example, it is 
not a good approximation to neglect the velocity terms except at the 
beginning and end of the arm motion [Ref, 33,36], 

Another simplification includes the ignoring of actuator 
dynamics. When actuator dynamics are considered in manipulator 
dynamics, higher-order differential systems result. This high order 
system is not readily solvable [Ref, 29; page 71], Therefore, 
dynamic analysis is greatly simplified if actuator and rigid 
manipulator lint dynamics are separated in the simulation approach. 
The effects of actuator dynamics is considered beyond the scope of 
this thesis. 

Computational reductions using model reformulations have 
also been presented which use various matrix, vector and numeric 
analysis techniques. These methods are discussed in the sections 
which follow. 

E, THE LAGRANGE FORMULATION 

The first manipulator dynamics formulations were based on 
Lagrange equation derivations [Ref, 29] and were so inefficient that 
torques for specific trajectories could not be computed in anything 
close to real-time requirements. Although this is a serious problem 
in manipulator control, Lagrange equations were one of the principal 
methods of simulation and analysis for almost a decade [Ref, 29], 
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Througli persistent efforts to increase computational efficiency, 
Lagrange equations now provide tlie foundation for current, more 
effective real-time control metliods. Discussions of Lagrangian 
equations can be found in most dynamics textbooks [i.e. Ref. 37]. 
Tbe Lagrange formulation can represent tbe dynamic behavior of rigid 
body systems. Kahn was tbe first to apply a general formulation of 
tbe Lagrange equation specifically to robot manipulators [Ref. 38]. 
Dicker adapted Eabn's work for open chain manipulator linkages, using 
homogeneous coordinates and the transformation matrices discussed 
earlier [Ref. 24] . 

The chief advantage of Lagrange formulated manipulator equations 
is in the straightf oxrward and simple methodology, since the six 
manipulator links are each consecutively referenced to the preceeding 
link. Lagrangian generalized coordinates thus lead to a systematic 
representation of multilink mechanisms. 

1 • Energy Calculations 

The Lagrange manipulator model formulation begins with the 
formulation of kinetic energy (KE) and potential energy (PE) 
expressions [Ref. 24]. 

a. Kinetic Energy. 

The critical difference between the Lagrangian-Euler and 
Newton-Euler methods comes in the method used to formulate kinetic 
energy. As previously stated in equation (8), a position vector to a 
point (for example a differential mass) in base coordinates, with 
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respect to link j when i=0 (for the base) can be written = 

or more simply, r = T. Velocity can then be written: 

3 



V = 



dr 

dt 



i 3T. . 

5 






T. •’r, 
J k 



(11) 



and 



r 2 = 



[sr — - »< 



r r } 



( 12 ) 



where : 





r j j 9T. 


3T. 1 


T 




r r — i X 


1 




= tr 


2 2 a,, 'j ('j) 

t-1 P-1 1 







(13) 



= joint variables, the generalized coordinates signifies the 
joint angle, 6., for single degree-of-freedom rotary joints 
and displacement of sliding joints* (Multiple degree-of- 
freedom joints are modeled as single degree-of-f reedom joints 
with intermediate links of zero length and mass*) 



Since kinetic energy = mv^, then the kinetic energy 

j. 



dKEj of a differential mass at point is: 
dKE. « i tr (•^r('*r.)^) dm 

J 2 J 
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i j T 

In the above, a trace operator forms the tensor product r from 

which the inertia matrix J. is found* 

J 

The total kinetic energy KE^. of a link is found by 



integrating over all the differental masses in all the links and 
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summing each link^s kinetic energy KE^ with respect to the inertial 



frame : 



KE. = J f dKE. = J tr ( T. / '^r^) dm tT ) 

J 2 J j 2 j j 



where : 



2 ' j ’*j j ^ 



• “i X 

J. = f(^T *^r ) dm = the inertia matrix 
3 
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(18) 



J. = inertia tensor with respect to the joint situated next 
to link j expressed in j's coordinates 



J. = m. 
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kj^.^ = radius of gyration of link j about the j-k axes 
m^ = link j mass 



T. = transformation matrices representing both linear and 
^ angular link velocities* 



Therefore, the manipulator structure's kinetic energy is: 
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Tlie actuator kinetic energy at the joints is: 



=tla. 4? 
actuator. 2 J j 
J 



Summarizing, the total kinetic energy is: 
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i=l 



b. Potential Energy, 



The potential energy of a individual link can be 
expressed as [Ref, 19^21] : 

PE = mgh = -m g , r (23) 

where _ 

r = position of the link's center of mass 
g = the gravity vector Igl = 9,8062 m/s* 
m = mass 
h = height 



For a link with a mass center at a position vector 

with respect to link j coordinate frame T. is: 

J 

T i- 
PE = -m. g T. 

where: ^ J J 



(24) 



^r. = a vector from link j origin to its center of gravity 

= the link j mass 
J 

The total robot arm potential energy is the sum of each 
link's potential energy PE^ expressed in the base coordinate frame: 
n n 



PE 



= } f dPE. 
j=l 



3 “ m. T. ■’r. 

^ J J J 

j=l 
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2. Lagrange Eonation Formulation 



After the Lagrangian, L = KE - PE, is established it is 
applied to the Lagrange manipulator formulation to produce the 
generalized force required for joint i to drive the ith robot arm 
link. Symbolically, we can define the forces as: 



f . 
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(26) 



The dynamic equations of motion for a n link manipulator as 
derived for the formulation of the Lagrangian function are [Ref, 30]: 
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(27) 



This dynamics equation is a closed form expression, and shows 
that an applied joint torque is explicitly dependent on all joint 
movements. The equation's complexity comes from the dynamic 
interactions between each of the manipulator links. 

For purposes of the following discussion, it is important to 
recognize that there are three types of terms in the Lagrangian 
dynamic equation. They are: 



(1) inertial forces which are proportional to the joint 

accelerations 4<T 
k 



38 



(2) velocity forces which are proportional to the product 4^.4 

p 

a. centripetal forces when k = p and proportional to 4^j, 
b* Coriolis forces when k f p, 

(3) gravity forces 

3 • Lagrange Matrix Formulation 

The manipulator arm dynamics of equation (27) can be 

rewritten in matrix-vector form [Ref. 6,39]: 

n n n 

} 'ijl ^ G. 

j=l j=l k=l 

or, more compactly (adding the effects of outside forces): 

F(t) = H(q)q* + C(q,q)q + G(q) + K(©)^k (29) 

In the Lagrangian formulation, the dynamic model parameters for the 
above are [Ref. 3,6,20]: 

H(q) = Inertial coefficient (n z n) matrix 
C(q,q) = Centrifugal and Coriolis force n-vector 
6(q) = Gravitational force n-vector 

K(q) = Jacobian matrix of joint forces created due to 
external forces exerted on link n 

k = n vector of external force exerted on link n 

4. Limitations in the Lagrange Formulation 

The dynamics computation using the Uicker Lagrangian 
formulation, equation (27), is far too slow for use in real-time 
control or in simulation. Luh, Walker, and Paul found that 7.9 
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seconds was required for each evaluation of the Dicker Lagrangian 

[Ref. 27] for a sir link manipulator. As can be seen from equation 

(27), a triple summation is evaluated for each joint torque to obtain 

the velocity product terms. Although it can be argued that the 

velocity products can be neglected [Ref. 19,21], another 

computational inefficiency exists in the double summations that must 

be computed at each joint for the acceleration terms. The double 

sums are: (1) the inside bracket summation, ranging from joints 1 to 

j, and (2) the outside bracket summation, ranging from joints i to n. 

4 

This results in computational operations proportional to n when a 
triple and double summation is considered with joint torque 
computation for n joints. Therefore, reformulation is necessary to 
satisfy real time control requirements. 

F. RECURSIVE LAGRANGIAN DYNAMICS 

In 1981, Hollerbach substantially reduced the Lagrangian method's 
computational requirements by establishing a simple, succinct 
formulation of (forward) link numbering recurrence relationships 
[Ref. 40]. Hollerbach's reformulations decreased the number of 
computational operations to the order of n. Additionally, Waters 
found generalized forces could be expressed in a manner that that 
took advantage of backward link numbering recurrence relations [Ref. 
29]. 

The structure of forward and backward recursions contributes 
greatly to increased computational efficiency. Each approach yields 
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equivalent equations, through (1) a backward recursive computation of 
the linear and angular velocities and accelerations from the base to 
the manipulator end, and (2) a forward recursive computation of 
generalized working forces and torques from end to base. Recursive 
dynamic equations can compute angular velocity, linear and angular 
acceleration, and forces and torques, and achieve great efficiency 
because they only execute recursions once, thus avoiding costly 
matrix summations. 

Hollerbach's reformulation also showed that the number of 
coefficients can be cut in half by using 3x3 transformation matrices 
and explicit position translations insteads of the 4x4 homogeneous 
transformations discussed earlier. There are many inefficiencies in 
4x4 matrices due to the many zeros, ones, and redundancies in 
original A, matrices. For purposes of this thesis, however, 4x4 
matrices are used. Greater computational efficiency has been 
sacrificed here for greater flexibility, and compatibility with many 
published research articles. 

As stated in section II. 5. b, the Lagrange formulated equation for 
obtaining generalized forces for an nrlink manipulator is: 
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where : 
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Hollerbach showed that to obtain generalized forces, the 
computational procedure requires: 

(1) computation and storage of the terms 

J 

(2) computation and storage the 3^Tj/9qj^3qj^ terms 

!• Lagrangian Dynamics with Backward Recursion 

The derivation of the generalized forces is: 
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The initial conditions for the base, the linear and angular 

• • • 

velocities and accelerations T and T , are zero, since the 

o o 



manipulator base does not move. 
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2. Forward Recursive Lagrangian Dynamics 



The next step in the recursion formulation clarifies how 
forces/torques are reflected back from manipulator tip to base. This 
forward recursion leads to increased Lagrangian dynamics efficiency. 



Since: T, = A. . . A, ,. . . .A, and 

j 1+1 1+2 j 
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The genralized force equation (6) becomes: 
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Substituting the and c^ terms into equation (36), the forces are; 
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This recursive formulation is computed by: 

(1) computing terms from i=l to i=n using equation (39) 



(2) computing and terms from i=n to i=l. 
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The second step is where computational complexity of an n 

4 

link arm is reduced from the order of n to n. The only way to 
further reduce this linear complexity would be to reduce the number 
of linear polynomial coefficients, which will not be done for the 
generalized program used in this thesis. 

Although a recursive Newton-Euler formulation is almost three 
times more efficient that a recursive Lagrangian formulation [Ref. 
40], there appears to be no fundamental difference between the two 
formulations when properly configured kinematically [Ref. 41]. 



Although not as efficient. 


the Lagrangian 


formolation has been 


selected 


here because it 


offers 


Straightforward 


programming 


algorithms 


which do not 


depend 


on 


Specific 


manipulator 



configurations. 
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III. MOTION MODELLING AND SIMULATION 



A. MODELLING AND SIMULATION APPROACH 

The modelling approach in this thesis is a Lagrangian description 
of a manipulator arm^s dynamic behavior. The model computer program 
utilizes a general-purpose kinematic analysis algorithm which 
includes both forward and backward recursive Lagrangian solutions. 
ITiese algorithms are based on coordinate frame representations for 
the links and formal Lagrangian mechanics interpretations. The use 
of generalized algorithms allows future evaluation of many different 
dynamic models within one common software program using a data base 
of various arm parameters. Additionally, a generalized program can 
be simplified for specific manipulator configurations. 

The individual control torques used in this program are provided 
in the form of input data. A given control force is used to compare 
the simulated dynamic response against a known experimental response. 

For each time step, two tasks are involved in the simulation: (1) 
the robot arm trajectory is determined through integration of 
acceleration and velocity for assumption of constant acceleration, 
and (2) the joint accelerations for the next time step are obtained 
through solution of the dynamic equations of motion. 



45 



B. COMPUTATION CONSIDERATIONS 



1* Simplifying Assiimptions 

Because of equation complexity, past researchers have used 
simplifying assumptions for the mechanical model so that a solution 
could be obtained [Ref. 29 ; page 62]. This program includes all 
terms which appear in the Lagrangian formulations as shown in 
equations (27) and (32). 

2. Computational Efficiency 

Current research emphasizes computational efficiency to allow 
a real-time dynamic equations solution for control schemes [Ref. 271. 
Additionally, increased computational efficiency promotes simulation 
cost optimization. The emphasis of this program is to provide the 
foundation for future control work and to offer as general a model as 
possible. Therefore, the program was not designed as the most 
computationally efficient, although efficient software schemes were 
used where possible. Hollerbach has concluded that 4,388 
multiplications and 3586 additions are required for the formulation 
selected for use in this program. Reductions to 2,195 
multiplications and 1,719 additions could be made using 3x3 
transformation matrices [Ref. 40] . 

3 . Structure Flexibility 

All manipulators are flexible to some extent. However, the 
rigid-body dynamics presented here are an idealization for a real 
manipulator. [Ref. 43] 
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4* Integration 



Currently, the standard procedure to solve a nonlinear system 
is to use an iterative numerical time integration method on the set 
of nonlinear differential equations describing the system [Ref. 44]. 
The thesis program as currently designed will analyze a rigid link 
manipulator using a trapezoidal munerical time integration algorithm 
[Ref. 45]. Through the assumption of constant joint acceleration 
during time steps, no iteration is necessary. 

5 . Frictional Forces 

Frictionless conditions were assumed for the thesis 
simulation. 

6. Vibration and Dynamic Stabiltv 

Vibrational and system dynamic stability effect were not 
considered in this thesis work. 

C. PROGRAM APPROACH 

The FORTRAN 77 language was chosen to implement the dynamic 
formulation. The source code produced for this program was complied 
on an IBM 3033 computer using the VS FORTRAN compiler (Release 3.0). 
The methodology and programming philosophy used in generating the 
source code are provided in the sections which follow. 

1. Principal Program Matrices 

In robot arm modeling, five terms (inertia, Coriolis, 
centrifugal, gravity, and external) make up the joint forces or 
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moment s 



Equation (29) can be rewritten to model the dynamics of a 



revolute six-element open chain linkage in the following manner: 

X = H(e)e + c(e,e)e + G(e) + K(e)^k (40) 

where : 

H(6) n X n symmetic, nonsingular moment of inertia matrix 

C(6,6) n X n matrix of centrifugal and Coriolis effects 

G(6) n X 1 vector specifying the effects due to gravity 

t(0) 6 X n Jacobian matrix of torques created at each 

joint due to external forces exerted on link n 

T 

K(6) transpose of K(6) 

k 6x1 vector of external forces exerted on link n 

X n X 1 vector of torques of each joint actuator* 

0 n X 1 vector of joint variables 

Note in the above that the joint torques are linear functions of the 
joint accelerations* 

2* Modelling Solution Methodology 

The methodology used to validate the dynamic model is shown 
graphically in Figure 6* Here, the torque was given for a specific 
instant in time* This was then combined with the present 
experimental angular position and velocity as input for equation 
(40)* External forces other than gravity where not considered with 
the input data available* The angular acceleration for the same time 
instant was then computed from equation (40)* A simple integration 
was then performed over the acceleration time domain to provide a 
velocity and position check with actual data* Figure 7 provides a 
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Figure 6. Model Formulation Diagram 
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Figure 7. Computer Program Flow Diagram 
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block diagram of tbe steps (and associated subroutines) which were 
used to implement this methodology. 

3 . Simulation Solution Methodology 

The methodology used to simulate the dynamic motion is shown 

graphically in Figure 8. As before, the stepped torque was provided, 

with only the initial velocity and position. The time interval 

between knots was divided into equally sized steps of length At. The 

acceleration at step i was assumed equal to that at i+1 and a 

standard forward finite difference formulation was used to determine 

the estimated velocity and position, 6.,- and 6..-, at time t._^. . 

^ 1+1 i+1 1+1 

These values were again applied to equation (40) to recompute the 
acceleration at time for next time step. This acceleration 

was then reapplied to the finite difference formulas to determine the 
next positions and velocity, etc. The step size was varied to 

determine the effects on computer run times and accuracy. 

4. Principal Subroutines 

The following descriptions apply to subroutines found in a 
subroutine called ROBARM. ROBARM was called by a main program as 
often as necessary to evaluate the particular dynamics at a specific 
knot. The recursive Lagrangian representation of the dynamic motion 
equations [Section II. F] was used in the simulation method developed 

by Walker and Orin [Ref. 3] to create a FORTRAN subroutine named 

• . • 

ARMl . ARMl allows input of position 0 , velocity 0 , acceleration 0 , 
external forces/moments, and joint forces and torques. 
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6iT«m 




Figure 8. Simulation Formulation Diagram 
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The FORTRAN subroutine, ARMl(6,d,6, k,r) , ultimately computed 

• • • 

H, the inertia term matrix, given 0,6,6 and the forces and moments 

• • 

exerted on link n, k. Another FORTRAN subroutine, ARM2(6,6,r), is 

• • • 

identical to ARM1(6,6,6, k,r) except that programming code for 
velocity terms, gravitational effects and external forces/moments 
effects, has been eliminated. Subroutines ARMl and ARM2 form the 
basic programs used for simulating the rigid body manipulator 
dynamics. An explicit analytic expression for each of the terms of 
motion equations is not required for the simulation. 

5. Determination of Acceleration 

A bias vector, b, is set equal to the torques due to gravity, 
centrifugal and Coriolis accelerations, and external forces and 
moments on link n. Therefore, 

b = c(e,e)e + G(e) + i(e)^ k (4i) 

Thus, the joint variable accelerations can be obtained by solving a 

linear matrix equation 

• • 

H(e)6 = (t - b). (42) 

The bias vector b is computed by setting 6, 6 and k to their current 

• • 

state, and letting 6=0. ARMl is then called in the following 
manner: ARM1(6,0, 0,k,b) . Given these inputs, the bias vector b is 
computed in ARMl. 

The difficult part in solving the above linear equation is in 
the matrix H element evaluations. This is done by setting 6 to its 
current state. ARM2 is then called in the following manner: 



53 



ARM2(6, e . ,li. ) « Here, e. is a n x 1 vector with the jth element equal 
«3 1 *3 

to 1, all other values of e are equal to 0* The variable is the 
jth column of H and represents the joint actuator torque when the 
joint velocities are zero* When the H matrix elements aredetermined, 
the joint accelerations can be obtained by solvingequation (42). 

6. Program Flow 

Summarizing, the computational procedures are: 

MAIN PROGRAM (process trajectories and torques) 

SUBROUTINE CONST 
SUBROUTINE TRAJ 

« « « 

SUBROUTINE ROBARM (Knot #,e,6,e,T,k,PLYLD, t) 

SUBROUTINE KINEMT (0,PLYLD) 

SUBROUTINE ARMl (e,6,0,k,B) 
i = 1 — > 6 

SUBROUTINE ARM2 (e,e,H) 

I = T - B 

SUBROUTINE TDDSOL (H,6, I, error status) 

• • • 

SUBROUTINE VELSOL (t, 0,0, Knot #) 

• • • 

SUBROUTINE POSSOL < t, 0,0,0, Knot #) 

• • • 

SUBROUTINE ROBSIM (Knot #,e,e,e,r,k,PLYLD, t) 
where subroutine: 

CONST ^ Loads constants, including: radii of gyration, masses, arm 
offsets, differential matrices, gravity constants. 

TRAJ • Loads trajectory data, either from point-to-point information 
or a fitted curve. Includes time, torque, velocity, and position. 
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ROBARM - Central umbrella subroutine, called by main program as each 
trajectory point is processed. Variable PLYLD allows program to 
adjust for playload weights and variable t is a time interval (sec), 

KINEMT ~ Calculates the link and transformation matrices, 

ARMl - Subroutine which normally calculates link torques, given link 
position, velocity, acceleration and external forces, however in this 
simulation method, was used to calculate the inertia matrix terms, 

ARM2 - Same as ARMl, with Coriolis, gravity, and coupling terms 
removed, 

TDDSOL - Linear simultaneous equation solver using trianglur 
decomposition method, 

VELCTY - Integration subroutine used to solve for velocity given 
acceleration, using the trapezoidal method, 

POSSOL ~ Integration subroutine to solve for position using the 
Hermitian form, 

ROBSIM “ Finite difference simulation subroutine, 

7* Program Algorithm Development 

The maximum number of links allowed in this program is 
fifteen (15), Thus, matrix maximum dimensions are configured by row, 
column, and link number. For example, the dimensioning of the link 
coordinate matrix is A(4,4,15), There are no serious limitations 
with these dimensions when using a large capacity computer (i,e, IBM 
3033 system) , 

The maximum number of knots which can evaluated is 
arbitrarily set at one hundred (100), This number can be changed 
significantly, without seriously overloading memory allocations. 

The specific numerical values and equations used to develop 
the computer program are provided below. 
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a. Subroutine CONST 



(1) DATA Input . The relative link masses (kg) for PUMA 

ARM 600 are: 



33.5 

77.3 

36.3 
8.95 
2.39 
1.00 J 



the radii of gyration (cm*) are: 



k* = 

XX 



■ 451.0 ■ 




■ 451.0 ■ 




■ 57.9 • 


565.7 




1847.0 




1408.0 


672.8 


k* = 


679.1 


k* = 


36.0 


31.6 


yy 


21.1 


zz 


31.6 


6.9 




11.2 




6.9 


. 33.8 . 




. 33.8 . 




.911. 



the first moments (cm) are: 



X = 



■ 0.0 • 




■ 0.0 • 




■ 8.0 • 


-21.6 




0.0 




21.75 


0.0 

0.0 


y = 


0.0 

2.0 


z = 


21.6 

0.0 


0.0 




0.0 




0.0 


. 0.0 . 




. 0.0 . 




. 1.0 . 



and the link parameters for the revolute arm are: 



•-90.0 • 




1 




■ 0.0 • 




■ 0.0 ■ 


0.0 








0.0 




43.2 


90.0 

-90.0 


0 = 


«3 


d = 


12.5 

43.2 


a = 


1.9 

0.0 


90.0 








0.0 




0.0 


. 0.0 . 




e. J 




. 0.0 . 




. 0.0 . 
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(2) Calculations ^ Recalling equation (19) 



the above 



data is used to compute the Inertia matricies: 



J. = 

J 



m . 
J 



2 2 2 

k. +k. -k. 

jyy jzz jxx 



2 

k. 

jxy 



2 

k. 

jxy 



2 

k. 

JXZ 



2 2 2 

k. -k. +k. 

JX3C jyy JZZ 



2 

k. 



2 

k. 

jyz 



2 2 2 

k. +k. -k. 

jyy JZZ 



X. 

J 



jyz 









z . 
J 



(19) 



(3) Programming Philosophy ^ The physical and geometric 

data is loaded from a disk file by the CONST subroutine. The 

calculation in this subroutine is only called once, as values therein 

remain unchanged during all conditions. The data includes: the 

number of links; the type of link (prismat ic=[l] or 

translational=[2] ) ; the Denavit-Hartenberg a, d, a, 0 parameters, the 

radii of gyration; the centers of mass; and the link masses. The 0 

(zero) matrix is then loaded and parameter a is converted to radians. 

The 0 matrix is used in subroutine ROBARM. 

In addition to computing and filling the matrix 

and r vectors, the A and T matrices are initialized to zero. The A 

o 

and T matrices are filled as identity matrices, 
o 
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b. Subroutine KINEMT 



(1) Calculations , The link coordinate, first and second 
partial matrices are calculated using equation (1), repeated below: 






cos 



6. I -sin 6. cos a. I sin 6. sin a. I a. cos 6. 



I 



sin 6. I cos 6, cos a. I -cos 6. sin a. | a. sin 6 
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j 

sin a 



■j 



I 



cos a. 



j 



I 



( 1 ) 





■-sin 
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1 
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J J 
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0. 

J 


I -sin 0. 
1 J 
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1 

1 


sin 0. sin a, 
J J 


30. 

J 
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1 

1 


0 






1 
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0 




0 




1 


0 






1 


0 




1 

o 
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« 


0 
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1 
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1 
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I -a. sin 0. 
I J J 

I a. cos 6. 

, J j 



I 



0 
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(43) 



0 

0 



(44) 



Using the above, is calculated using equation (2) 



and equation (33): 



T. 

J 



= = 



J 

n A 

i=l 



T. , A. 
J-1 J 



(2) Programming Philosophy . The subroutine is called by 
ROBARM at each knot. The calculations preformed in this subroutine 
are dependent on the value of link 'variables* and change at each 
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knot calling. In the cases discussed in this thesis, all the 

revolute 'Variables^' are 6. The constant physical and geometric 

data are passed from the CONST to the KINEMT subroutines via a COMMON 

statement (ROB2,ROB3). These parameters are used to compute all of 

the A, 3A/36 and 3^A/30^ matrices for links 1 to 6. As previously 

stated, these and other 4x4 matrices are configured in the program as 

by [row, column, link number]. 

Upon determination of the A matrices, the values of 

T^, Tg, T^, Tj, and Tg are computed using Equation (2). 

Finally, the gravity vector is loaded so that gravity 

acts in the positive z direction. All these vectors and the above 

o 

matrices are passed to other programs via a COMMON statement 
(ROB8,ROB9). 



c. Subroutine ARMl 

(1) Calculations . Using equation (34) and equation 



(35), Tj , Tj and are computed from j=l to j=6: 

. . 3A. . 

T. = T. , A. + T. , — i 0. 

J J-1 J J-1 30 J 

j 



(45) 



dA 



T. = T , A, + 2 T. , 
J J-1 j J-1 



30 



i 9j * Tj., 
j 



3»A, . 



3A. .. 



0? + T. 



30. » 
J 



. . e. 

30. J 
J 



(46) 



Additionally, 



3T. 
I 

30. 






3A. 
I 

30. 

J 



J 



(47) 
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When T. , T. , and T. are computed, D. and c. is 
J J J 11 

computed from j=6 to j=l using equation (37) and equation (38): 

j 



D. = J. + A . D 
J J J J+1 J+1 



and 



c. 

J 



= m . 



r. + A. .. c. _ 
3 J+1 J+1 



where : 





■ 0 • 








■ 0 • 


1 
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II 






1 J 




1 J 




1 J 




■ 0 • 




■ 0 • 




■ 0 ■ 
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y. 
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0 


6 


0 


II 

M 


0 


'5 = 


Z 5 


'6 = 






. 1 . 




1 J 




1 J 



The torques in a revolute manipulator arm are then computed by 
rewriting equation (39) : 



r . = tr 
3 



3T. 
i 

96. 

J 



D. 



9T 
T j 

- g — ^ c. 



(48) 



96 



j 



(2) Matrix Multiplication and Addition Subroutines . The 
following subroutines are called during ARMl and ARM2 to perform 
repeated multiplication and additions on indexed 4x4 and 4x1 
matrices : 



MAT44 - Multiples a 4x4 matrix and a 4x4 matrix 

MAT44T - Multiples a 4x4 matrix and a 4x4 Transposed matrix 

MATC4 - Multiples a Constant and a 4x4 matrix 

MATCl - Multiples a Constant and a 4x1 matrix 

MAT41 - Multiples a 4x4 matrix and a 4x1 matrix 

ADD44 - Adds a 4x4 matrix to another 4x4 matrix 

ADDALL - Adds 4 different 4x4 matrices 

ADDll Adds a 4x1 matrix and a 4x1 matrix 

FIRST - Finds the trace of two multiplied 4x4 matrices to find 
the left (first) side of the force equation (48). 
SECOND Multiplies a 1x4 matrix and a 4x4 matrix, and 

subsequently another 4x1 matrix to find the right 
(second) side of the force equation (48). 
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(3) Programming Philosophy . ARMl and ARM2 contained many 
short subroutine calls which are used to multiply and added matrices* 
A variety of real constant arrays are declared for use in the ARMl 
and ARM2 programs (Cl to CIS). These area provided to ''hold'' 
preliminary calculated data for further use by other subroutines. 
The use of many rather than few holding matrices is based on the lack 
of memory constraints, and the avoidance of the need to constantly 
reinitialize. The use of these holding matrices can be avoided by 
rewriting the matrix multipling and adding subroutines so there is no 
requirement for holding. 

The first terms calculated are T, from j=l to 6. 

When j=l, the left side of equation (48) vanishes leaving only 
the right side. This condition is satisfied using an ''IF'' loop. 

Similiar equation ''reduction'' conditions exist for 

• • 

the T, D, , C., and 3T./30. matrices. The D. and C. matrices are 
i 1 J J 1 i 

filled in reversed order (backward recursion), 
d. Subroutine ARM2 

. • 

(1) Calculations . Rewriting equation (35), is 
computed from j=l to j=6: 



• • * • 



3A. .. 

T = T. , A, + T. . — i e 



j 



j-1 j j-1 



30 



j 



(49) 



j 



When T., T. are computed, D, is computed from j=6 to j=l using 
J J ^ 

equation (38) : 



D. = J. *? + A D 
J j J J+1 J+1 
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The torques are finally computed using: 



T . 

J 



tr 




( 50 ) 



(2) Programming Philosophy , The memory requirements for 
ARM2 are less, since the velocity and gravity terms are removed. 

Furthermore, there is no requirement for calculation of T, since 

• 

is 0, and all velocity terms are removed, leaving all subsequent 
terms to equal 0. 



Additionally, the terms are not required as they 
are used with gravity terms in the force equation. The 
matrices are calculated in ARMl and their values remain unchanged 
from those used in ARMl. 

e. Subroutine TDDSOL 

The subroutine TDDSOL is an IMSL simultaneous equation 
solver called LEQIF. This subroutine provides a solution to linear 
equation generated by the inertia and bias matrices, using full 
matrices in virtual memory. The program listing is not provided for 
this copyrighted material (1982, by IMSL, INC.). The subroutine 
calls other IMSL subroutines. However, any linear equation solving 
subroutine, with capabilities to warn of matrix singularities can be 
used. Argument input for this subroutine was: 

Argument # 1 - Input n x n 'A'-matrix of equation AX=B 

2 - Rows in 'A^ as dimensioned by calling program 

3 - The order of matrix 'A' 

4 - Equal to n (related to program speed) 

5 - Right hand side of equation AX = B. 

at output, solution matrix X replaces B. 

6 - Rows in 'B' as dimensioned in calling program 
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7 - Number of columns in 'B' 

8 - 1=0, Code to factor matrix and solve equation AX=B 

9 - Real work area = 3*n 

10 - Error parameter IER=129 — > Matrix a singular AX=B 

f. Subroutine VELSOL 

Subroutine VELSOL was written as a simple adaption of 

trapezoidal rule [Ref, 47| page 75]. The subroutine provided the 

velocity solution by integrating angular acceleration over the entire 

time considered. The argument input for this subroutine was: 

Argument # 1 - Time, all time point covered to point of calling 

2 - Acceleration, submitted one link a time 

3 “ Velocity, output 

4 “ Link number 

g. Subroutine POSSOL 

Subroutine POSSOL is written using an adaption of a 
Hermite interpolation method of integration [Ref. 47j page 314]. 
This method provides a more accurate solution as derivatives are 
known. The argument input for this subroutine is: 

Argument # 1 - Time, all time point covered to point of calling 

2 — Acceleration, submitted one link a time 

3 - Velocity, from previous subroutine 

4 - Position, output 

5 - Link number 

h. Subroutine ROBSIM 

This subroutine implements the simulation technique 
described in section III.C.3. The simulation program is called by 
setting ISIM equal to one (ISIM=1) in the main program (ROBOT). When 
this subroutine is called, all output can be sent, via a separate 
subroutine, to the same position, velocity, and acceleration files as 
the modelling program. These files are for use with graphics 
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programs* This subroutine computes new position and velocity 

information, and calls ARMl and TDDSOL to determine the new 
acceleration at each step* Step sizes are determined by setting the 
number of steps in the main program (ISTEP = number of steps)* 
Torque is keep constant as the subroutine is called at each knot* 
Arguments passed to this subroutine are identical to those passed to 
ROBARM* Only the knot number, torque, and the time matrix are used 
throughout an entire program run. Arguments for position and 
velocity are used only on the first knot* 

D* DATA INPUT 

Discrete torque data is fed into the modelling and simulation 
process at each trajectory knot* An analysis of the relative joint 
moment magnitudes (except friction) was performed for the PUMA 
manipulator by R*P* Paul [Ref* 46]* Paul estimated the values for 
the link masses and radii of gyration based on a manipulator 
examination* The actual mass and radii of gyration values were 
obtained analytically, and were within an estimated 10% of actual 
values* Paul's results were used as input data for this simulation. 
This input data includes all robot link inertias, motor torque 
characteristics, and accuracies* The torque inputs and 

velocity/positional output data was obtained through use of feedback 
controller sensors, and its accuracy is estimated with 1% to 2%* 
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E. PROGRAM OUTPUT 



Simulation program output included the position, velocity, and 

acceleration versus time data for the end effector and each joint. 

This data vas placed in a data file which easily interfaced with 

a graphics interpreter program. Data output included: 

Desired Position vs Computed Position files (PEn DATA) 

Desired Velocity vs Computed Position files (VEn DATA) 

Desired Acceleration vs Computed Acceleration files (AEn DATA) 
Error evaluation files (ERROR DATA) 

Additionally, an independent subroutine (TRANSP) was developed to 

calculate the end-effector transposition matrix using the actual and 

computed position files. The transposition matrix was then used as 

output for this subroutine, providing the x,y,z coordinates of the 

end-effector. Data output included: 

Actual path x,y,z coordinates (PATHACTT DATA) 

Modelled path x,y,z coordinates (PATHMOD DATA) 

Simulation path x,y,z coordinates (PATHSIM DATA) 

This data was used to provide a graphical representation of the 
end-effector path. 
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IV. RESULTS AND DISCUSSION 



A. MODELLING PROGRAM RESULTS 

Based on the dynamic model formulation presented in this thesis, 
a general computer program package for the dynamic analysis of a 
revolute six joint manipulators was developed. 

The input torque for each joint was supplied as a series of 
trajectory knots, as shown in Figure 9 for a typical joint. The only 
arm external loading terms considered were those of gravity. Angular 
position, velocity, and acceleration, were subsequently computed and 
plotted against actual knot values to verify the accuracy of the 
model, as shown typically for joint 2 in Figures 10, 11, and 12 (A 
complete set of all graphs for torque, acceleration, velocity, and 
position are available in Appendix B, Figures 19-42.) 

Table I provides a numerical average modelling error for each 
link for the trajectory terms. This error was obtained * by 
determining the difference between the actual data and the model 
results, and then normalizing with the actual data. The errors were 
summed over all knots and averaged. 



Table I. Modelling Joint Position Errors 



LINK NUMBER 


1 


2 


3 


4 


5 


6 


Acceleration 


1 2.778% 


1.492% 


2.201% 


1.244% 


2.543% 


2.510% 


Max Error ( /sec^) 


1 0.152 


0.494 


1.542 


0.103 


1.222 


0.552 


Velocity 


1 6.457% 


4.404% 


3.091% 


2 . 824% 


5.397% 


4.283% 


Max Error ( /sec) 


1 2.306 


1.267 


2.050 


1.487 


3.055 


1.687 


Position 


I 7.210% 


8.156% 


5,979% 


9.640% 


13,386% 


5.763% 


Max Error ( ) 


I 10.871 


8.154 


13.926 


12.968 


13.899 


8.170 
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Figure 9, Joint 2 - Torque vs Time 
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Figure 10. Joint 2 Trajectory Verification - Angular Acceleration vs Time 
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Figure 11. Joint 2 Trajectory Verification - Angular Velocity vs Time 
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Figure 12* Joint 2 Trajectory Verification ~ Angular Position vs Time 



After all trajectory knots had been analyzed by the main program, 
the predicted joint model positions were supplied to another 
independent computer program TRANSP (similiar to KINEMT) , where the 
angular positions of each link were transposed, from base to end- 
effector, to determine the position vector of the end-effector. The 
motion produced in each plane, XY, XZ, and YZ, was then plotted 
graphically, as seen in Figures 13-15, Table II. provides the 
average error achieved while the arm moved over the specified path. 
Again, this error was obtained by determining the difference between 
the actual data and the model results, normalizing with the actual 
data, and again averaging over the range of knots. The errors 
translate to a maximum path variation of approximately 12 cm. 



Table II, End-effector Modelling Errors 



Knots # 


X coord 


Y coord 


Z coord 


Avg Error 


22.72% 


12.25% 


11.08% 


Max Error 


27.4cm 


4,1cm 


7.1cm 



B, SIMULATION PROGRAM RESULTS 

The use of the simulation subroutine was bypassed (ISIM=0) during 
the above operations. This was done to allow modelling without the 
the simulator program. When the simulation subroutine was 
incorporated into the program (ISIM=1), several simulations, using 
varying stepsizes, were evaluated for the series of knot points. 
Torques were held constant during simulations between knot points. 
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Figure 13. End-effector Path - X vs Y Coordinate 



o 
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Figure 14* End~effector Path - X vs Z Coordinate 
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Figure 15. End-effector Path - Y vs Z Coordinate 



In addition to noting errors achieved during each evaluation, the 
time required to run the simulator program on the IBM 3033 was also 
recorded. This information is provided in Table III and Figure 16. 



Table III. CPU Time Required to Process Simulation Program (Seconds) 



Knots 




Number of 


Steps Processed between Knots 


Processed 


5 


10 


15 


20 


25 


30 


3.0 


1.53 


3.03 


3.43 


6.01 


6.37 


9.06 


5.0 


1.84 


4.08 


6.65 


9.61 


12.10 


17.30 


10.0 


3.34 


8.22 


14.10 


20.20 


26.40 


37.40 


15.0 


4.57 


12.20 


20.20 


30.60 


40.50 


60.30 


20.0 


5.74 


17.50 


26.80 


38.30 


50.80 


79.10 


25.0 


7.17 


20.20 


33.00 


48.00 


63.00 


99.20 


30.0 


8.43 


24.00 


39.40 


57.30 


75.20 


125.20 


35.0 


9.69 


28.50 


45.40 


68.50 


88.00 


138.20 


37.0 


10.40 


29.80 


48.30 


71.50 


94.30 


152.30 



Once again, acceleration, velocity, and position outputs were 
obtained and the results were recorded graphically, as shown in 
Appendix B. The trajectory information for joint 1 is provided in 
Figures 17-19, other results are included in Appendix B. 



C. DISCUSSION 
!• Model 

The most important result achieved was that the program 
demonstrated the capabilities to generate a forward dynamic solution 
and serve as an accurate dynamic robot model. Linear programming 
techniques were used to determine the angular acceleration. This 
program was successfully tested against known results for a given 
trajectory of the PUMA arm. 
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Figure 16* Simulator Program Mainframe Runtimes 
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Figure 17. Joint 1 Trajectory Simulation - Angular Acceleration vs Time 
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Figure 18. Joint 1 Trajectory Simulation - Angular Velocity vs Time 
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Figure 19, Joint 1 Trajectory Simulation - Angular Position vs Time 



The acceleration data supplied was determined analyt icallyi 
however, the method used to determine this data was not available. 
The small acceleration errors achieved gives rise to a suspicion that 
the analytical process may also have included joint torque 
information, possibly processed through a dynamic formulation program 
similar to that presented in this thesis. 

The accuracy of the data which describes a robot's joint 
links, mass, to a set of general inverse kinematic equations was 
estimated at ±10% [Ref. 46]. The physical property data was 

originally obtained by ''fitting'' the manipulator arm's mass and 
inertia terms to a point where they are consistent with the robot's 
motions. The error achieved by the model appears to be well within 
results which could be expected from the data provided. 

It was noted that the model end-effector path tracked 
erratically in all knots after 15 seconds. The source of these 

errors can be seen by inspecting the angular position verses time 
figures for each joint, and observing that the individual joint 
errors increase at times after 15 seconds. Although at worst these 
errors are relatively small (7“8%), they accumulative towards the 
end-effector. This is due to the nature of process involved in 
transposing end-effector coordinates to base coordinates. 
Consequently, this produces a 12-13% error in end-effector position. 
This error seems reasonable, considering the low magnitude of 
individual joint errors. 
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The thesis display program (TRANSPLT) was capable of 
displaying motion data in a two dimensional plane and provided an 
acceptable, but limited method of visualizing robot motion in order 
to evaluate robot performance. 

2. Simulation 

The accuracy of the 6 link simulation program deteriorated 
rapidly after five seconds of simulated performance. At that time 
the accelerations oscillated rapidly about the actual values, with 
magnitudes greater than 500%. Velocity and positional results were, 
consequently, several orders of magnitude different than expected 
results. In an effort to eliminate the possibility that excessive 
errors in kinematic variables near the end-effector were creating 
large errors in the first three links, the masses of links 4, 5, and 
6 were combined arithmetically, and the program was rerun as a four 
link problem. While acceleration results did not change under this 
second evaluation trial, velocity and positional data were generally 
as expected for the first 5 seconds. Typical velocity results are 
shown in Figure 18. The remainder of the disscussion will address 
the second (four link) evaluation. 

A detailed examination was made to determine the nature of 
the oscillations for a segment between two knots. At the beginning 
of each segment, a new torque value is supplied to the simulator. 
This torque value does not change further until the next segment is 
evaluated. While velocities and positions form an integral relation 
with accelerations, they are not treated as such in the non-iterative 
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constant acceleration approximation scheme implemented in the thesis 
simulation routine* Consequently^ as each succeeding step is 
evaluated, the method produces velocities and positions which become 
increasingly more inaccurate as the time evaluation proceeds. Since 
these values are supplied to the matrix equation for the 
acceleration, it can be seen that the acceleration errors will 
increase as velocity and position errors compound. 

Values in the inertia matrix, which are supplied as the left 
side of the matrix dynamic equation, were examined at all evaluation 
points in the belief that the deviations in acceleration may have 
resulted from singularities produced in a sparsely occuppied matrix. 
However, singularity conditions were not observed. 
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V 



A. MODEL 

A computer program was written which allowed the calculation of 
open spatial mechanical manipulator chain dynamics based on the 
application of independent equations represented in Hollerbach's 
recursive Lagrangian form. The resulting dynamic formulation was 
developed in numerical matrix form rather than as specific equations 
of motion. The model was evaluated for the PUMA robot manipulator. 

In view of the inaccuracies in model constants it is difficult to 
evaluate the accuracy of calculated variables. However, the model 
did produce results of sufficient accuracy that it could provide 
insight into the dynamic properties of the manipulator arm, and could 
give assistance in the task of trajectory planning, 

B, SIMULATION 

The simulation routine did not produce reliable or accurate 
results due to the programs' inability to continuously provide values 
of velocity and position which would satisfy their integral relation 
with acceleration. The linearizing approximation of stepwise 
constant acceleration was a poor approach. 
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C. FUTURE WORK 



1. Program Enhancements 

The simulation routine must be rewritten to iterate about 
each acceleration step until accurate values of velocity and position 
are obtained for the next step* 

An entire robot arm system can be modelled by: (1) modelling 
of an operational amplifier which can convert the position error 
signal into a current to drive a servo valve, (2) modelling a 
hydraulic actuator system (which can be a second order model for a 
flow control valve, and an integrator to model the actuator), and (3) 
connecting the output of the hydraulics system (torque) to the input 
for the thesis model in order to predict joint accelerations, 
velocity, and position. 

The robot modelling could be modified to accommodate other 
manipulators with Euler or cylindrical coordinates. Additionally, 
although the discussion in this thesis deals primarily with open 
chain systems, the program can be readily extended to treat closed 
loop systems. 

2. Computers 

Future enchancements of the thesis program could include 
better user interfacing (^'user friendly'^), more efficient code, 
better utilization of memory, and improved input/output . 
Additionally, three dimensional kinematics representations be 
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designed by interfacing the program with CAD/CAM or finite element 
software • 

Microcomputer (Personal Computer) solutions to applications 
problems will become a common practice among manufacturing engineers 
in the coming years* It is recommended that the FORTRAN program be 
compiled and run on a microcomputer, which could ultimately can be 
used to control an actual manipulator arm. Memory allocation might 
become a serious problem if this thesis program is transferred to a 
smaller micro- or mini-computer system. A benefit of using a 
dedicated mini- or microcomputer would be the decreased dependence on 
time-shared mainframe computer time. 

Additionally, the program can be modified for double 
precision to increase accuracy, part icularily around suspected 
singularity points, at an expense of increased total computation 
time • 

3. Physical Analysis 

The physical and geometric parameters of robot arm components 
are perhaps the most difficult to obtain. In addition to published 
data, generalized methods are available which can be used to obtain 
these values. The program can be used to analyze the physical 
parameters of small laboratory manipulator arms by varying dynamic 
parameters estimates to match known motion data. Such a model can be 
coupled directly to a robot arm control programs, where results for 
each of the major arm linkages and control components can be 
validated experimentally. 
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The modelling program can be used to evaluate the relative 
effects of acceleration and velocity on various terms in the dynamic 
equations. 

4. Control Design 

In its present form, the program permits development of robot 
and workspace models, robot motion programming, and duty cycle 
calculations, such as forces, torques, and deflections. 

When using the program to design workcells by producing two- 
dimensional diagrams of manipulator motion, work points can be 
defined, and a motion path can be analyzed to allow collision 
avoidance evaluations. 

The simulation can be used to determine the effects of 
variables on dynamic performance, to develop robot dynamic analysis 
data, and to identify and evaluate robot singularity conditions. 
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APPENDIX A 



RULES GOVERNING LINKS, JOINTS, AND THEIR PARAMETERS 
!• Mechanical manipulators consist of a sequence of rigid bodies, 
called links, connected by either revolute or prismatic joints. Each 
joint-link pair constitutes one' degree of freedom. 

2. An n degree-of-f reedom manipulator has n. joint-link pairs with 
link 0. Link 0 is not considered part of the robot 

3. Link 0 is attached to a supporting base where an inertial 
coordinate frame is established. Last link can have a tool. 

4. Joints and links are numbered outward from the base 

5. Each link is connected to two others at most so that no closed 
loops are formed. 

6. A joint axis (for joint i) is established at the connection of 
two links. This joint axis has two normals connected to it, one for 
each link. 

7. The relative position of two such connected links (link i-1 and 
link i) is given by d^, the distance measured along the joint axis 
between normals. 

8. The joint angle, 6^, between the normals is measured in a plane 
normal to the joint axis. They determine the relative position of 
neighboring links. 
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9» A link i is connected to two other links 



Two joint axes are established at both ends of connection. 

Links maintain fixed configuration between their joints. 

Configuration structure is characterized by a. and a. 

11 

d^ and 0^ determine the neighbor link relative positions 
10. There are a,^ a., d. and 0. parameters associated with each 
manipulator link and they represent the minimal sufficient set to 
determine complete kinematic configuration of each robot arm link, 
a^ = length , shortest distance along common normal between joint axes 
“ twist , angle between joint axes in plane perpendicular to a^. 
d^ = distance between adjacent links. 

0£ = angle between adjacent links. 



DENAVIT-HARTENBERG REPRESENTATION 
The Denavit-Hartenberg representation is a matrix method of 
systematically establishing a coordinate system (body-attached frame) 
for each link of an articulated chain. A 4x4 homogeneous 
transformation matrix represents each link's coordinate system at the 
joint with respect to the previous link's coordinate system. 

An orthonormal Cartesian coordinate system is established for 
each link at its joint axis^ plus the base coordinate frame. Since a 
rotary joint has only one degree of freedom, each robot arm 
coordinate frame corresponds to joint (i+1) and is fixed in link i. 
When an actuator activates joint i, link i moves with respect to link 
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(i-1)* Since i's coordinate system is fixed in link i, it moves with 
link i« 

The nth coordinate frame moves with the tool or hand, (link n) • 

The base coordinates are defined as the 0th coordinate frame 

(z ,y ,z ) which is also the inertial robot arm coordinate frame. 

0*^0 o 

Every coordinate frame is determined and established by three 
rules : 

1. the axes lies along the motion axis of ith joint 

2. the X. axis is normal to the axis, pointing away 

3. the y^ axis completes the righthand coordinate system 

The location of coordinate frame 0 may be choosen anywhere in 

supporting base, as long as the axis lies along the motion axis of 

first joint. The nth frame can be placed anywhere in hand as long as 

the X axis is normal to the z . axis, 
n n-1 

The Denavit-Hartenberg representation of rigid links depend on 
four geometric quantities associated with each link that completely 
describe any revolute/prismatic joint. 



1. 6. = joint angle from x. axis to x. axis about z. . 

^ axis (Right Hand Rule) ^ ^ 

2. d. = distance from (i-l)th coordinate frame origin to 

intersection of z. . axis with the x. axis along 

4 . 1 . • 1 

the axis. 

3. a^ = offset distance from intersection of axis with 

X. axis to origin of ith frame along x. axis (or 

shortest distance between z. . and z. axes. 

1-1 1 

4. = offset angle from axis to z^ axis about x^ 

axis (Right Hand RuieJ ^ 
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For a rotary joint, d^, a^, and are joint parameters and 
remain constant for a robot* 6^ is a joint variable that changes 
when link i moves (rotates) with link i-1* For a prismatic joint, 

0^, a^, and are joint parameters and remain constants for a robot* 
d^ is joint variable* 

PROCEDURE FOR ESTABLISHING CONSISTENT ORTHONORMAL COORDINATE SYSTEMS 
Given: n^degree-of-f reedom robot arm 

Establish: an orthonormal coordinate system to each robot arm link* 

1. Est9bli$h ,Qooi:4ioate sTstgio . 

Establish a righthand orthonormal coordinate system 
at supporting base with axis lying along joint 1 motion axis* 

2. I9.it 90d lOQP . 

For each i, i=l,***n, perform steps 3-6 

3. Establish joint axis . 

Align with joint i+1 motion axis 

4* Establish origin of ith coordinate , system . 

Locate origin of ith coordinate system at intersection of and 

z, . axes or at interestion of common normals between z, and z. . 
i-1 i 1-1 

axes and z. axis* 
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5# Establish x . axis » 

^ ±(z . . X z . ) 

Establish x. = ^ — 

^ I |z X z . I I 
1-1 1 

or along common normal between axes when they are 

parallel • 



6. Establish y . axis , 

^ (^i X x^) 

Establish y, = 

^ N^i ^ ^i I I 

to complete righthand coordinate system. 



7* Find joint and link parameters . 



For each i, i=l,,,,,n, perform steps 8-11, 



8. Find d . 



d^ is distance from (i-l)th coordinate system origin to 

intersection of z . . axis and x. axis along z. . axis. It is 
1-1 1 1-1 

joint variable if joint i is prismatic. 



9, Find a. 



a. is distance from intersection of z . ^ axis and x. axis to ith 
1 1-1 1 

coordinate system origin along x^ axis. 



10, Find 6 . 



6. is rotation angle from x. . axis to x. axis about z. . axis, 
1 1-1 1 1-1 

It is joint variable if joint i is rotary. 



11. Find g. 



a. is rotation angle from z, . axis to z, axis about x. axis. 
1 i-1 i 1 
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Once the Denavit--Hartenberg representation coordinate system is 



established for each link: 

A Homogeneous tranf ormation matrix, developed 

(Relates ith coordinate frame to (i“l)th coordinate system) 

1. rotate about z. . axis an angle of 6. to align x. . 

1-1 1 1-1 

axis with x. axis (x. . axis is parallel to x.) 

1 1-1 1 

2m translate along axis a distance of d^ to bring 

X, . and X. axes into coincidence. 
i-1 1 

3. translate along x^ axis a distance of a^ to bring 
two origins into coincidence 

4. rotate about x^ axis an angle of to bring the two 
coordinate systems in coincidence. 

Each operation is expressed by a basic homogeneous rotation or 
translation matrix, and the product of 4 basic homogeneous 
transformation matrices 

This yields composite homogenous tranf ormation matrix, 
known as the Denavit-Hartenberg transformation matrix for adjacent 
coordinate frames. 
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TORQUE, ACCELERATION, VELOCITY, AND POSITION VS TIME CURVES FROM JOINT 1 TO 6 
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APPENDIX C 



A. COMPUTER PROGRAMS 



C***********«* *************************************************** 

c*****************************************************************main 

(;******************************************************** *********]t(;^JIs) 

COMMON /ROBl/ G,PI,D2R,R2D 

COMMON /ROB2/ ERTIA(4,4,15) ,RBAR(4,15) 

COMMON /ROB3/ SMALLA(15) ,SMALLD(15) ,ALPHA(15) 

COMMON /ROB4/ B2XX(15) ,B2YY(15) ,B2ZZ(15) 

COMMON /ROBS/ AMASS(IS) ,ACTIA(15) , LINKS 
COMMON /ROB6/ XBAR(IS) ,YBAR(15) ,ZBAR(15) 

COMMON /ROB7/ GRAV(4,15) ,LNKTYP(15) 

COMMON /ROB8/ A(4, 4,15) ,PA(4,4, 15) ,PPA(4,4,15) 

COMMON /ROB9/ ZERO(15) ,TT(4,4,15) ,PTQ(4,4,15) 

COMMON /ROBIO/ POS(15,100) ,VEL(15,100) , ACC(15,100) 

COMMON /ROBll/ DELTA(15,100) ,DELTV(15,100) ,DELTP(15,100) 

COMMON /ROB12/ SIMERR(15) ,TOTACC(15) ,ACCSIM(15,100) , ISTEP,KNOTS 
COMMON /ROBl 4/ POSSIM(15,100) , ISIM 

REAL T(15),TD(15),TDD(15),TAD(15),FMK(15),TBETA(15,100),THETD(15,1 
100) ,THDT2(15,100) ,TD(15,100) ,TIME(100) ,TS(15) ,TDS(15) ,TDDS(15) ,PAY 
2LD 

INTEGER I, J, LINKS 

ISTEP=5 

ISIM=1 

CALL CONST 

C LOAD trajectory KNOTS 

CALL TRAJ (KNOTS, TIME,THETA,THETD,THDT2,TD) 

C INITIALIZE PASS ON MATRICES 

DO 10 1=1,15 
T(I)=0.0 
TD(I)=0.0 
TDD(I)=0.0 
FMK(I)=0.0 
10 CONTINUE 

C CALL SUBROUTINE TO COMPUTE ACCELERATIONS 

DO 30 J=l, KNOTS 

PAYLD=0.0 

DO 20 1=1, LINKS 

FMK(I)=0.0 

TAU(I)=TU(I,J) 

T(I)=THETA(I,J)*D2R 
TD( I) =THETD( I, J ) *D2R 
TDD( I ) =THDT2 ( I , J ) *D2R 
20 CONTINUE 

CALL ROBARM (J,T,TD,TDD,TAD,FMK,PAYLD,TIME) 
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CALL OUTPUT (J,T,TD,TDD,THETA,THETD,THDT2,TIME,PAYLD) 

30 CONTINUE 

c find model errors 

CALL ERRORS (KNOTS, THETA, THETD,THDT2) 

C FIND SIMULATION ERRORS 

STOP 

END 



(]***** *********************«**********************************«**J(QQjy^ 

(]****************************** **********************************F0B ARM 
0********** ***************************** **********************'***F0B ARM 

SUBROUTINE ROBARM (KN,T,TD,TDD,TAU,FMZ, PAYLD,TIME) 

COMMON /ROBl/ G,PI,D2R,R2D 

COMMON /ROB2/ ERTIA(4,4,15) ,RBAR(4,15) 

COMMON /ROB3/ SMALLA(15) ,SMALLD(15) ,ALPHA(15) 

COMMON /ROB4/ B2XX(15) ,B2YY(15) ,B2ZZ(15) 

COMMON /ROBS/ AMASS ( 15 ) ,ACTIA( 15) , LINKS 
COMMON /ROB6/ XBAR(15) ,YBAR(15) ,ZBAR(15) 

COMMON /ROB7/ GRAV(4,15) ,LNKTYP(15) 

COMMON /ROB8/ A(4, 4,15) , PA(4, 4,15) , PPA(4,4,15) 

COMMON /ROB9/ ZERO(15) ,TT(4,4,15) ,PTQ(4,4,15) 

COMMON /ROBIO/ POS(15,100) ,VEL(15,100) ,ACC(15,100) 

COMMON /R0B14/ POSSIM(15,100) , ISIM 

REAL T(15),TD(15),TDD(15),TAU(15),FMK(15),HJ(15),B(15),EJ(15),H(15 
1,15) ,WORK(45,3) ,TIME(100) ,TDOT2(100) ,TDOT(100) ,TPOS(100) ,PAYLD 
INTEGER I,J,ISTAT,KN 
CALL KINEMT (T,PATLD) 

DO 10 1=1,15 
B(I)=0.0 
10 CONTINUE 

CALL ARMl (T,TD,ZERO,FMK,B) 

c ^load ej vector 

DO 40 1=1, LINKS 
DO 20 J=l, LINKS 
HJ(J)=0.0 
EJ(J)=0.0 
20 CONTINUE 

EJ(I)=1.0 

CALL ARM2 (T,EJ,HJ) 

DO 30 J=l, LINKS 
H(J,I)=HJ(J) 

30 CONTINUE 

40 CONTINUE 

DO 50 1=1, LINKS 
TDD(I)=TAU(I)-B(I) 

50 CONTINUE 

IER=0 

C COMPUTE LINK ACCELERATIONS 

CALL TDDSOL (H,15, LINKS, LINKS, TDD,15, 1,0, WORK, lER) 
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C ^load vectors into perm matrices 

DO 80 LN=1, LINKS 
TDD(LN)=TDD(LN) *R2D 
ACC(LN,KN)=TDD(LN) 

DO 60 1=1, KN 
TDOT2(I)=ACC(LN,I) 

TDOT(I)=0.0 



60 CONTINUE 

CALL VELSOL (TIME,TDOT2,TDOT,KN) 

C CALCULATE VELOCITY 

VEL(LN, KN) =TDOT( KN) 

TD(LN)=TDOT(KN) 

DO 70 1=1, KN 
TDOT(I)=VEL(LN,I) 

70 CONTINUE 

C CALL POSSOL (TIME,TDOT,TDOT2,TPOS,KN) 

CALL VELSOL (TIME,TDOT,TPOS, KN) 

Q CALCULATE POSITION 

POS (LN, KN) =TPOS( KN) 

IF (KN.EQ.l) TINIT=T(LN) *R2D 
T( LN) =TPOS ( KN) +TINIT 
80 CONTINUE 

0 call simulation 

IF (ISIM.NE.l) GO TO 90 

CALL ROBSIM (KN,T,TD, TDD, TAU,FMK,PAYLD, TIME) 

90 CONTINUE 



RETURN 

END 
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« 4i 4i 4i # * « « « 4i « « 4i « « « « 4i ♦ « « 4i 4e ♦ 4i ♦ 4i ♦ « « ♦ « 4 ♦ 4i ♦ ♦ 4i # 4i ♦ 4i ♦ ♦ 4i ♦ ♦ ♦ 4i 4i ♦ ♦ 4i ♦ 4t « 4i 4i 4t * ♦ 4i 4 4 

(;;«***«*««« 4t 4t ««««««««♦♦««««♦««♦« ««♦««««♦«««««« ♦««♦««««« 4( ««♦♦««««♦«« 

SUBROUTINE ARM! (T,TD,TDD,FMK,TAU) 

COMMON /ROBl/ G,PI,D2R,R2D 

COMMON /ROB2/ ERTIA(4,4,15) ,RBAR(4,15) 

COMMON /ROB3/ SMALLA(15) ,SMALLD(15) ,ALPHA(15) 

COMMON /ROB4/ B2XX(15) ,B2YY(15) ,B2ZZ(15) 

COMMON /ROBS/ AMASS(IS) ,ACTIA(15) , LINKS 
COMMON /ROB6/ XBAR(15) ,IBAR(15) ,ZBAR(15) 

COMMON /ROB7/ GRAV(4,15) ,LNKTYP(15) 

COMMON /ROBS/ A(4,4,15) ,PA(4,4,15) ,PPA(4,4,15) 

COMMON /ROB9/ ZERO(15) ,TT(4,4,15) ,PTO(4,4,15) 

REAL T(15),TD(15),TDD(15) ,TAU(15) , FMK(15) , DD(4, 4,15) , CC(4, 15) , Q (4 

1.4.15) ,C2(4,4,15),C3(4,4,15),C4(4,4,15) ,C5(4,4,15),C6(4,4,15) ,C7(4 

2.4.15) ,C8(4,4,15),C9(4,4,15),C10(4,4,15),C11(4,4,15),C12(4,4,15),C 

313(4,4,15),C14(4,15),C15(4,15),C17,C19,TD1(4,4,15),TD2(4,4,15) 

INTEGER I,J 

C INITALIZE ALL CONSTANT MATRICES 

DO 30 1=1,4 
DO 20 J=l,4 
CC(I,J)=0.0 
a4(I,J)=0.0 
a5(i,j)=o.o 
DO 10 K=l, LINKS 
DD(I,J,K)=0.0 
TD1(I,J,K)=0.0 
TD2(I,J,K)=0.0 
C1(I,J,K) =0.0 
C2(I,J,K) =0.0 
C3(I,J,K)=0.0 
C4(I,J,K)=0.0 
C5(I,J,K)=0.0 
C6(I,J,K)=0.0 
C7(I,J,K)=0.0 
C8(I,J,K)=0.0 
C9(I,J,K) =0.0 
C10(I,J,K)=0.0 
C11(I,J,K)=0.0 
C12(I,J,K)=0.0 
C13(I,J,K)=0.0 
10 CONTINUE 

20 CONTINUE 

30 CONTINUE 
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LOAD T MATRIX 



C 

C . . PA. 

C T = T A + T J Q 

C J J-1 J J-1 P Q J 

C J 

C 

C = Cl + C3 

C 

C TD1/TD2 MATRICES INITIALIZED TO ZERO IN CONST SUBROUTINE 



40 

50 

60 

C 

C“ 

C 

C 

C 

C 

C 

C" 

C 

C— 



DO 60 J=l, LINKS 
IF (J.EQ.l) GO TO 40 
CALL MAT44 (TDl, J-1, A, J, Cl, J) 
(TT,J-1,PA,J,C2,J) 
(TD(J),C2,J,C3,J) 
(C1,J,C3,J,TD1,J) 



CALL MAT44 
CALL MATC4 
CALL ADD44 
GO TO 50 
CONTINUE 
CALL MATC4 
CONTINUE 
CONTINUE 



(TD(J),PA,J,TD1,J) 



T = T 



J-1 



C4 



A + 2 T 
J J-1 



P A 



P Q 



Q + 
J 



J-1 



C7 



2 

P A 

P Q 2 
J 

C9 



• • 

LOAD T MATRIX 

.2 PA.. 

Q + T J Q 

J J-1 P Q J 

J 



+ Cll 



DO 90 J=l, LINKS 

IF (J.EO.l) GO TO 70 

CALL MAT44 (TD2, J-1, A, J, C4, J) 

CALL MATC4 (2.0,TD1,J-1,C5,J) 

CALL MAT44 ( C5, J,PA, J, C6, J) 

CALL MATC4 (TD( J) , C6, J, C7, J) 

CALL MAT44 (TT, J-1, PPA, J, C8, J) 

CALL MATC4 (TD( J) **2, C8, J, C9, J) 

CALL MAT44 (TT, J-1,PA,J,C10,J) 

CALL MATC4 (TDD( J) , CIO, J, Cll, J) 

CALL ADDALL ( C4, J, C7 , J, C9, J, Cll, J,TD2, J) 
GO TO 80 
70 CONTINUE 

CALL MATC4 (TD( J) **2,PPA, J, C9, J) 

CALL MATC4 (TDD(J) ,PA, J, C11,J) 

CALL ADD44 ( C9, J, Cll, J,TD2, J) 

80 CONTINUE 

90 CONTINUE 
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LOAD CONSTANT MATRICES 



C-- 

C 

C 

C 

C— 

C 

C-- 



DO 140 11=1, LINKS 
1=7-11 

..T 

D = J T + A D 
III I+l I+l 



LOAD CONSTANT D MATRIX 



= C12 



Cl 3 



100 

110 

C 

C 

C 

C 

C 

C 

C 



IF (I.EQ.LINKS) GO TO 100 
CALL MAT44T (ERTIA, I,TD2, I, C12, I) 
CALL MAT44 (A, I+1,DD, I+l, C13, I) 
CALL ADD44 ( C12, I, C13, I,DD, I) 

GO TO 110 
CONTINUE 

CALL MAT44T (ERTIA, I,TD2, I, DD, I) 
CONTINUE 



LOAD CONSTANT C MATRIX 



C = M R + A C 
I II I+l I+l 



Q4 



CIS 



120 



IF (I.EQ.LINKS) GO TO 120 
CALL MATQ (AMASS, I, REAR, I, Q4, 1) 
CALL MAT41 (A, I+l, CC, I+l, CIS, I) 
CALL ADDll (C14,I,C1S,I,CC,I) 

GO TO 130 
CONTINUE 

CALL MATCl (AMASS, I, REAR, I, CC, I) 

CONTINUE 

CONTINUE 




LOAD PARTIAL T WRT Q 



ISO 



160 

170 



DO 190 J=l, LINKS 

IF (J.EQ.l) GO TO ISO 

CALL MAT44 (TT, J-1, PA, J, PTQ, J) 

GO TO 180 

CONTINUE 

DO 170 11=1,4 

DO 160 JJ=1,4 

PTQ(II,JJ,J)=PA(II,JJ,J) 

CONTINUE 

CONTINUE 
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180 CONTINUE 
190 CONTINUE 



C CALC FORCE/ TORQUE 

C I P T I T P T 

C F = TR I 1 D I - G 1 C 

Cl IpqiI PQI 

C III I 

C = C17 - C19 

C 



DO 200 1=1, LINKS 

CALL FIRST (PTQ,I,DD, C17) 

CALL SECOND (GRAY, PTQ, I, CC, Q9) 
TAU(I)=C17-a9 
TAD(I)=TAU(I)/10000.0 
200 CONTINUE 
RETURN 
END 
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Q * ********************************************************** *******^^^^}/[2 
Q******************************************************************/^T^2 



Q^**t:t**^***** ********************************************* *****^**^^J^^ 



SUBROUTINE ARM2 (T,TDD,TAU) 

COMMON /ROBl/ G,PI,D2R,R2D 

COMMON /ROB2/ ERTIA(4,4,15) ,RBAR(4,15) 

COMMON /ROB3/ SMALLA(15) ,SMALLD(15) ,ALPHA(15) 

COMMON /R0B4/ B2XX(15) ,B2Yy(15) ,B2ZZ(15) 

COMMON /ROBS/ AMASS(IS) ,ACTIA(15) , LINKS 
COMMON /ROB6/ SBAR(15) , YBAR(15) ,ZBAR(15) 

COMMON /ROB7/ GRAV(4,15) ,LNKTYP(15) 

COMMON /ROBS/ A(4,4,15) ,PA(4,4,15) ,PPA(4,4,15) 

COMMON /ROB9/ ZERO(15) ,TT(4,4,15) ,PTQ(4,4,15) 

REAL T(15) ,TDD(15),TAU(15) ,DD( 4, 4,15) , C4(4, 4, 15) , C10(4, 4,15) , Cll (4 
l,4,15),a2(4,4,15),C13(4,4,15),TD2(4,4,15),C17 
INTEGER I,J 

C INITALIZE CONSTANT MATRICES 

DO 30 1=1,4 
DO 20 J=l,4 
DO 10 K=l, LINKS 
DD(I,J,K)=0,0 
TD2(I,J,K)=0.0 
C4(I,J,K)=0.0 
C10(I,J,K)=0.0 
ai(I,J,K) =0.0 
C12(I,J,K)=0.0 
C13(I,J,K)=0.0 
10 CONTINUE 

20 CONTINUE 

30 CONTINUE 

C 

C LOAD STRIPPED T MATRIX 

C 

C.« .. .PA .. 

C T = T A + T J Q 

C J J-1 J J-1 P Q J 
C J 

C 

C = C4 + Cll 

C 



40 

50 



DO 60 J=l, LINKS 
IF (J.EQ.l) GO TO 40 
CALL MAT44 (TD2, J-1, A, J, C4, J) 
CALL MAT44 (TT, J-1,PA, J, QO, J) 
CALL MATC4 (TDD( J) , CIO, J, Cll, J) 
CALL ADD44 (C4, J,C11, J,TD2, J) 

GO TO 50 
CONTINUE 

CALL MATC4 (TDD( J) , PA, J,TD2, J) 
CONTINUE 
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LOAD CONSTANT D MATRIX 



60 CONTINUE 

C 

C ..T 

C D=JT + A D 
C I I I I+l I+l 

C 

c = C12 + as 

c 

DO 90 11=1, LINKS 
1=7-11 

IF (I.EQ.LINIS) GO TO 70 
CALL MAT44T (ERTIA, I,TD2, 1, Q2, 1) 

CALL MAT44 (A, I+l, DD, I+l, CIS, I) 

CALL ADD44 ( C12, I, CIS, I,DD, I) 

GO TO 80 
70 CONTINUE 

CALL MAT44T (ERTIA, I,TD2, I,DD, I) 

80 CONTINUE 

90 CONTINUE 

C CALC FORCE/ TORQUE 

C I P T I 

C F = TR I 1 D I 

C I I P Q I I 

C ill 

C 

C = C17 

C 

DO 100 1=1, LINKS 

CALL FIRST (PTQ, I,DD, C17) 

TAU(I)=C17 

TAU(I)=TAU(I)/10000.0 
100 CONTINUE 

C 

RETURN 

END 
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(;;«««««♦♦««««««««««««««««««««««««««««««««*««««««««««««««««♦« *«««««*QU*ppUX 



Q*«* *«***************««««*****««****«*««****««*****««*** **«««* «««*Q^<ppU<p 

SUBROUTINE OUTPUT ( J,T,TD, TDD, THETA, THETD,THDT2, TIME, PAYED) 

COMMON /ROBl/ G,PI,D2R,R2D 

COMMON /ROB2/ ERTIA(4,4,15) ,RBAR(4,15) 

COMMON /ROB3/ SMALLA(15) ,SMALLD(15) ,ALPHA(15) 

COMMON /ROB4/ B2XX(15) ,B2YY(15) ,B2ZZ(15) 

COMMON /ROBS/ AMASS(IS) ,ACTIA(15) , LINKS 
COMMON /ROB6/ XBAR(15) ,YBAR(15) ,ZBAR(15) 

COMMON /ROB7/ GRAV(4,15) ,LNKTYP(15) 

COMMON /ROBS/ A(4,4,15) ,PA(4,4,15) ,PPA(4,4,15) 

COMMON /ROB9/ ZERO(15) ,TT(4,4,15) ,PTQ(4,4,15) 

COMMON /ROBIO/ POS(15,100) ,VEL(15,100) ,ACC(15,100) 

COMMON /ROBll/ DELTA(15,100) ,DELTV(15,100) ,DELTP(15,100) 

COMMON /ROB12/ SIMERR(IS) ,TOTACC(15) , ACCSIM(15,100) , ISTEP,ZNOTS 
COMMON /ROB13/ T2(15,100) ,TLAST(15) ,TDLAST(15) ,TDDLST(15) 

COMMON /ROB14/ POSSIM( 15, 100) , ISIM 

REAL T(15),TD(15),TDD(15),THETA(15,100),THETD(15,100),THDT2(15,100 
1),TIME( 100), PAYED 



INTEGER I, J, KNOTS, ISIM 
C WRITE (27,40) J, PAYED, TIME(J) 

C ACCELERATION 

C WRITE (27,50) 

DO 10 K=l, LINKS 

DELTA( K, J ) =ABS ( THDT2 ( K, J ) -TDD( K) ) 

C WRITE (27,80) K,THDT2(K,J) ,TDD(K) ,DELTA(K,J) 

10 CONTINUE 

C VELOCITY 

C WRITE (27,60) 

DO 20 K=l, LINKS 

DELTV ( K, J ) =ABS ( THETD( K, J ) -TD( K) ) 

C WRITE (27,80) K,THETD(K, J) ,TD(K) ,DELTV(K, J) 

20 CONTINUE 

C POSITION 

C WRITE (27,70) 

DO 30 K=l, LINKS 

DELTP( K, J ) =ABS (THETA( K, J) -T( K) ) 

C WRITE (27,80) K,THETA(K, J) ,T(K) ,DELTP(K, J) 

30 CONTINUE 

IF (ISIM.EQ.l) GO TO 40 



WRITE (41,80) 
WRITE (42,80) 
WRITE (43,80) 
WRITE (44,80) 
WRITE (45,80) 
WRITE (46,80) 



OUTPUT TO GRAPHICS DATA FILES 

TIME(J),THDT2(1,J),TDD(1) 

TIME(J),THDT2(2,J),TDD(2) 

TIME(J),THDT2(3,J),TDD(3) 

TIME(J) ,THDT2(4, J) ,TDD(4) 

TIME(J) ,THDT2(5,J) ,TDD(5) 

TIME(J),THDT2(6,J),TDD(6) 
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WRITE (31,80) TIME(J),THETD(1,J),TD(1) 
WRITE (32,80) TIME(J) ,THETD(2, J) ,TD(2) 
WRITE (33,80) TIME(J) ,THETD(3, J) ,TD(3) 
WRITE (34,80) TIME(J) ,THETD(4, J) ,TD(4) 
WRITE (35,80) TIME(J) ,THETD(5, J) ,TD(5) 
WRITE (36,80) TIME(J) ,THETD(6, J) ,TD(6) 

Q 

WRITE (21,80) TIME(J),THETA(1,J),T(1) 
WRITE (22,80) TIME(J) ,THETA(2, J) ,T(2) 
WRITE (23,80) TIME(J) ,THETA(3, J) ,T(3) 
WRITE (24,80) TIME( J) ,THETA(4, J) ,T(4) 
WRITE (25,80) TIME(J) ,THETA(5, J) ,T(5) 
WRITE (26,80) TIME(J) ,THETA(6, J) ,T(6) 
GO TO 70 



C~ 

40 



50 



70 

40 

80 

90 

100 

110 



CONTINUE 

IF (J.NE. KNOTS) GO TO 70 

WRITE (27,90) ISTEP, KNOTS 

DO 50 11=1, LINKS 

SIMERR( II) =SIMERR( II) /T0TACC( II) 

WRITE (27,100) II,SIMERR(II) 

CONTINUE 

WRITE (21,110) TIME(J),THETA(l,J),POSSIM(l,J) 

WRITE (22,110) TIME(J),THETA(2,J),POSSIM(2,J) 

WRITE (23,110) TIME(J),THETA(3,J),POSSIM(3,J) 

WRITE (24,110) TIME(J),THETA(4,J),POSSIM(4,J) 

WRITE (25,110) TIME(J),THETA(5,J),POSSIM(5,J) 

WRITE (26,110) TIME(J) ,THETA(6,J) ,POSSIM(6,J) 

CONTINUE 

RETURN 

FORMAT (/13HKN0T NUMBER =,I5,10H PAILOAD =,F15.7,8H TIME = 
1 ) 

FORMAT (3F15.7) 

FORMAT (/17HNUMBER OF STEPS =,I5,18H KNOTS PROCESSED =,I5) 
FORMAT (/13HLINK NUMBER =,I4,10H ERROR = ,E18.8) 

FORMAT (3F15.7) 

END 



,F13.5 
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(;;***********«*********«*****«********************«**********4t***«(;;Q{43j 

(]************************* o*************** ***•*««*«********** 

SUBROUTINE CONST 

COMMON /ROBl/ G,PI,D2R,R2D 

COMMON /R0B2/ ERTIA(4,4,15) ,RBAR(4,15) 

COMMON /ROB3/ SMALLA(15) ,SMALLD(15) ,ALPHA(15) 

COMMON /R0B4/ B2XX(15) ,B2YY(15) ,B2ZZ(15) 

COMMON /ROBS/ AMASS ( 15 ) ,ACTIA( 15) .LINKS 
COMMON /ROB6/ XBAR(15) ,YBAR(15) .ZBAR(15) 

COMMON /ROB7/ GRAV(4,15) .LNKTYPdS) 

COMMON /ROBS/ A(4,4,15) ,PA(4,4.15) .PPA(4.4,15) 

COMMON /ROB9/ ZERO(15) ,TT(4,4,15) ,PTO(4,4,15) 

c***************************«*****load universal constant 

PI=ARSIN(1.0)*2.0 

D2R=PI/180.0 

R2D=180.0/PI 

G=980.621 

ARM CHARACTERISTICS 

READ (15,*) LINKS,(LNZTYP(I).SMALLA(I),SMALLD(I),ALPHA(I),B2XX(I), 
1B2YY( I) ,B2ZZ(I) ,XBAR( I) , YBAR( I) ,ZBAR( I) .AMASS (I) , ACTIA( I) , 1=1. LINK 
2S) 

C **«*****«***********«**«**«*****£^Q^ Ljj^£ INERTIA'S 
DO 10 1=1. LINKS 

ERTIA( 1,1,1) =AMASS (I) • ( (B2YY( I) +B2ZZ ( I) -B2XX(I) ) / 2 . 0) 

ERTIA( 2 , 2. I) =AMASS ( I) *( (B2XX( I) +B2ZZ( I) -B2YY( I) ) /2 . 0) 

ERTIA( 3,3,1) =AMASS (I) *( (B2XK( I) +B2YY( I) -B2ZZ (I) ) /2 . 0) 

ERTIA( 1 , 4, I) =AMASS (I) *XBAR( I) 

ERTIA(2,4, I)=AMASS( I) *YBAR(I) 

ERTIA( 3,4,1) =AMASS ( I) *ZBAR( I) 

ERTIA( 4, 1 , I) =AMASS ( I) *XBAR( I) 

ERTIA(4,4,I)=AMASS(I) *1.0 
ERTIA(4,1,I)=AMASS(I) *XBAR(I) 

ERTIA(4,2,I)=AMASS(I) *YBAR(I) 

ERTIA( 4, 3 , I) =AMASS ( I) *ZBAR( I) 

ERTIA(1,2,I)=0.0 
ERTIA(1,3,I)=0.0 
ERTIA(2,1,I)=0.0 
ERTIA(2,3,I)=0.0 
ERTIA(3,1,I)=0.0 
ERTIA(3,2,I)=0.0 
10 CONTINUE 

C *««**«******»**»**««****««******pj^]) Q matrix P link MASS CENTERS 
DO 30 1=1,4 
DO 20 J=l, LINKS 
RBAR(I,J)=0.0 
20 CONTINUE 
30 CONTINUE 

RBAR(3,1)=ZBAR(1) 

RBAR(4,1)=1.0 
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RBAR(1,2)=XBAR(2) 

RBAR(3,2)=ZBAR(2) 

RBAR(4,2)=1.0 

RBAR(3,3)=ZBAR(3) 

RBAR(4,3)=1.0 

RBAR(2,4)=YBAR(4) 

RBAR(4,4)=1.0 

RBAR(3,5)=ZBAR(5) 

RBAR(4,5)=1.0 

RBAR(3,6)=ZBAR(6) 

RBAR(4,6)=1.0 

C*«****«*«**»******************«**2ER0 and alpha — >RAD MATRICES 
DO 39 1=1,15 
ZERO(I)=0.0 

39 CONTINUE 

DO 40 1=1, LINKS 
ALPHA( I) =ALPHA( I) *D2R 

40 CONTINUE 

(;**********«**«**********«******j}^jjj^j2£ A MATRICES 
DO 80 J=l,4 
DO 70 K=l,4 
IF (J.EQ.K) GO TO 50 
A(J,K,1)=0.0 
TT(J,K,1)=0.0 
GO TO 60 
50 CONTINUE 

A(J,J,1)=1.0 
TT(J,J,1)=1.0 
60 CONTINUE 
70 CONTINUE 
80 CONTINUE 
RETURN 
END 
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C *«««**«*««******«******«************«**** *****«*****«**«*«****«£J]S{£]^ 
Q ********************************************************* 

Q *«****4>**************************************************«*****£]]i)£)ff 

SUBROUTINE KINEMT (T, PAYED) 

COMMON /ROBl/ G,PI,D2R,R2D 

COMMON /ROB2/ ERTIA(4,4,15) ,RBAR(4,15) 

COMMON /ROB3/ SMALLA(15) ,SMALLD(15) ,ALPHA(15) 

COMMON /R0B4/ B2XX(15) ,B2YY(15) ,B2ZZ(15) 

COMMON /ROBS/ AMASS(IS) ,ACTIA(15) , LINKS 
COMMON /ROB6/ XBAR(15) , YBAR(15) ,ZBAR(15) 

COMMON /ROB7/ GRAV(4,15) ,LNKTYP(15) 

COMMON /ROBS/ A(4,4,15) ,PA(4,4,15) ,PPA(4,4,15) 

COMMON /ROB9/ ZERO(15) ,TT(4, 4,15) ,PTQ(4, 4,15) 

REAL T( 15), PAYED 

C COMPUTE INDIVIDUAL LINK A-MATRICES 

DO 10 1=1, LINKS 
A(1,1,I)=C0S(T(D) 

A(l,2, I)=-COS(ALPHA( I) ) *SIN(T( I) ) 

A(l,3,I)=SIN(ALPeA(I))*SIN(T(I)) 

A( 1 , 4 , I ) =SMALLA( I) *COS ( T( I) ) 

A(2,1,I)=SIN(T(I)) 

A(2,2, I)=COS(ALPHA( I) ) *COS(T( I) ) 

A( 2, 3 , I) =-SIN( ALPHA( I) ) *COS(T( I) ) 

A( 2 , 4 , 1) =SMALLA( I) *S IN( T(I) ) 

A(3,1,I)=0.0 

A(3,2,I)=SIN(ALPHA(D) 

A( 3 , 3 , 1) =COS( ALPHA( I) ) 

A(3,4,I)=SMALLD(I) 

A(4,1,I)=0.0 
A(4,2,I)=0.0 
A(4,3,I)=0.0 
A(4,4,I)=1.0 
10 CONTINUE 

C COMPUTE AND LOAD 1ST PARTIAL A-MATRIX 

DO 30 1=1, LINKS 
PA(1,1,I)=-SIN(T(D) 

PA(1,2, I)=-COS(T(I) ) *COS(ALPHA( I) ) 

PA(1,3, I)=COS(T( I) ) *SIN(ALPHA( I) ) 

PA( 1 , 4, I) =-SMALLA( I) *SIN(T( I) ) 

PA(2,l,I)=COS(T(D) 

PA(2,2, I)=-SIN(T( I) ) *COS(ALPHA( I) ) 
PA(2,3,I)=SIN(T(I))*SIN(ALPHA(D) 

PA(2,4,I)=SMALLA(I)*COS(T(D) 

DO 20 K=l,4 
PA(3,K, I) =0.0 
PA(4,K,I)=0.0 
20 CONTINUE 
30 CONTINUE 
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c 



COMPUTE AND LOAD 2ND PARTIAL A-MATRIX 



DO 50 1=1, LINKS 
PPA(l,l,I)=-COS(T(D) 

PPA(1,2, I)=SIN(T( I) ) •COS(ALPHA( I) ) 

PPA(1, 3 , I) =-SIN(T( I) ) *SIN( ALPHA( I) ) 

PPA( 1,4,1) =-SMALLA( I ) *COS ( T( I) ) 

PPA(2,1,I)=-SIN(T(D) 

PPA(2,2, I)=-COS(T( I) ) *COS(ALPeA( I) ) 

PPA( 2, 3 , I) =COS(T( I) ) *SIN(ALPHA( I) ) 

PPA( 2, 4, 1) =-SMALLA( I) *SIN(T( I) ) 

DO 40 E=l,4 
PPA(3,E,I)=0.0 
PPA(4,E,I)=0.0 
40 CONTINUE 
50 CONTINUE 

C LOAD LINE TRANSPOSITION T-MATRICES 

DO 110 1=1, LINES 

C 

DO 100 J=l,4 
DO 90 E=l,4 
TT(J,E,I)=0.0 
DO 80 L=l,4 
IF (I.EQ.l) GO TO 60 

Tr(J,E,I)=TT(J,E,I)+TT(J,L,I-l)*A(L,E,I) 

GO TO 70 



60 


CONTINUE 




TT(J,E,1) 


70 


CONTINUE 


80 


CONTINUE 


90 


CONTINUE 


100 


CONTINUE 


L 

110 


CONTINUE 



Q load gravity matrix 

DO 120 1=1, LINES 
GRAV(1,I)=0.0 
GRAV(2,I)=0.0 
GRAV(3,I)=G 
GRAV(4,I)=0.0 
120 CONTINUE 
RETURN 
END 
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C*** ********************************************** **************«*x}^^ 
c* ********************************************************** 
C************* ******************************************** ********xr;^' 

SUBROUTINE TRAJ ( KNOTS, TIME, THETA, THETD,THDT2,TU) 

COMMON /ROB5/ AMASS(15) , ACTIA(15) ,LINKS 

COMMON /ROB13/ T2(15,100) ,TLAST(15) ,TDLAST(15) ,TDDLST(15) 

COMMON /ROB14/ POSSIM(15,100) , ISIM 

REAL THETA(15,100),THETD(15,100),THDT2(15,100),TU(15,100),TIME(100 
1 ) 

INTEGER I, J, KNOTS 
READ (20,*) KNOTS 
DO 20 1=1, LINKS 
DO 10 J=l, KNOTS 

READ (20,*) TIME(J),THETA(I,J),THETD(I,J),THDT2(I,J) 
T2(I,J)=THDT2(I,J) 

10 CONTINUE 
20 CONTINUE 

DO 30 J=l, KNOTS 
READ (50,*) TDUM 

READ (50,*) TU(1,J),TU(2,J),TU(3,J),TU(4,J),TU(5,J),TU(6,J) 

C WRITE (27,40) TDUM,TU(1, J) ,TU(2, J) ,TU(3, J) ,TU(4,J) ,TU(5,J) ,TU(6, J) 
30 CONTINUE 
C KNOTS=10 
RETURN 

C40 FORMAT (7F10.4) 

END 
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(•«**«**«««*«*****«*««**««*«**««*««**«**«**«**«««««****««**«*«« 
(]*********** ******************************************************£££0RS 

********************************** *«**«*««*«««4t**«**«*«*****£g£Q£g 

SUBROUTINE ERRORS ( KNOTS, THETA, THETD,THDT2) 

COMMON /ROB5/ AMASS( 15) ,ACTIA(15) , LINKS 

COMMON /ROBll/ DELTA(15,100) ,DELTV(15,100) ,DELTP(15,100) 

COMMON /ROB14/ P0SSIM(15,100) , ISIM 

REAL THETA(15,100),THETD(15,100),THDT2(15,100),DELA,DELV,DELP,DN,A 
1 A, AV, AP, MINA, MINV, MINP, MAXA, MAXV, MAXP, TDDSUM, TDSUM, TSUM 

C INITIALIZE VARIABLES 

DO 20 K=l, LINKS 

DELA=0.0 

DELV=0.0 

DELP=0,0 

TDDSUM=0.0 

TDSUM=0.0 

TSUM=0.0 

MAXA=0.0 

MAXV=0.0 

MAXP=0.0 

MINA=100.0 

MINV=100.0 

MINP=100.0 

C FIND max/ MIN 

DO 10 J=l, KNOTS 

IF (ABS(DELTA(K,J)) .GT.MAXA) MAXA=ABS(DELTA( K, J) ) 

IF (ABS(DELTV(K,J)) .GT.MAXV) MAXV=ABS(DELTV(K, J) ) 

IF (ABS(DELTP(K,J)).GT.MAXP) MAXP=ABS(DELTP(K, J) ) 

IF (ABS(DELTA(K,J)) .LT.MINA) MINA=ABS ( DELTA ( K,J) ) 

IF (ABS(DELTV(K,J)) .LT.MINV) MINV=ABS(DELTV(K, J) ) 

IF (ABS(DELTP(K,J)) .LT.MINP) MINP=ABS(DELTP(K, J) ) 

c find difference/ errors 

DELA=DELA+ABS ( DELTA( K, J ) ) 

DELV=DELV+AB S ( DELTV ( K, J ) ) 

DELP=DELP+ABS ( DELTP( K, J ) ) 

TDDSUM=TDDSUM+AB S ( THDT2 ( K, J ) ) 

TDSUM=TDSUM+ABS ( THETD( K, J ) ) 

TSUM=TSUM+ABS(THETA(K, J) ) 

10 CONTINUE 

AA=DELA/ TDDSUM 
AV=DELV/ TDSUM 
AP=DELP/TSUM 

WRITE (37,30) AA, MINA, MAXA 
WRITE (37,30) AV, MINV, MAXV 
WRITE (37,30) AP, MINP, MAXP 



20 


CONTINUE 




RETURN 


30 


FORMAT (3F18.7) 




END 
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************************************************** «********««**«{IQ3SIM 
********************************** ******************************30BSIM 
***************************************************** ******«**«*30BSIM 

SUBROUTINE ROBSIM (KN,T,TD,TDD,TAU,FMK,PAYLD,TIME) 

COMMON /ROBl/ G,PI,D2R,R2D 

COMMON /ROB2/ ERTIA(4,4,15) ,RBAR(4,15) 

COMMON /ROB3/ SMALLA(15) ,SMALLD(15) ,ALPHA(15) 

COMMON /ROB4/ B2XX(15) ,B2YY(15) ,B2ZZ(15) 

COMMON /ROB5/ AMASS ( 15 ) ,ACTIA( 15) , LINKS 
COMMON /ROB6/ XBAR(15) ,yBAR(15) ,ZBAR(15) 

COMMON /ROB7/ GRAV(4,15) ,LNKTYP(15) 

COMMON /ROBS/ A(4,4,15) ,PA(4,4,15) ,PPA(4,4,15) 

COMMON /ROB9/ ZERO(15) ,TT(4,4,15) ,PTO(4,4,15) 

COMMON /ROBIO/ POS(15,100) ,VEL(15,100) , ACC(15, 100) 

COMMON /R0B12/ SIMERR(15) ,T0TACC(15) , ACCSIM(15,100) , ISTEP,KNOTS 
COMMON /R0B13/ T2(15,100) ,TLAST(15) ,TDLAST(15) ,TDDLST(15) 

COMMON /R0B14/ POSSIM(15,100) ,VELSIM(15,100) ,ACLSIM(15,100) ,ISIM 
REAL T(15) ,TD(15) ,TDD(15) ,TAU(15) ,FMZ(15) ,HJ(15) ,B(15) ,EJ(15) ,H(15 
1,15) ,WORK(45,3) ,TIME(100) ,TDOT2(100) ,TDOT(100) ,TPOS(100) ,TDDSIM(15 
2) ,TDSIM(15) ,TSIM(15) ,TD0LD(15) ,DELTT,STEPSZ,PAYLD 
INTEGER I,J,KN,LN 
IF (KN.EQ. KNOTS) GO TO 120 
IF (ISIM.NE.l) GO TO 120 

C 

DO 30 LN=1, LINKS 
IF (KN.NE.l) GO TO 10 
TDDSIM(LN)=TDD(LN) *D2R 
TDSIM(LN)=TD(LN) *D2R 
TSIM(LN)=T(LN)*D2R 
GO TO 20 
10 CONTINUE 

TDDS IM( LN) =TDDLST(LN) 

TDS IM( LN) =TDLAST( LN) 

TSIM(LN) =TLAST(LN) 

20 CONTINUE 

30 CONTINUE 

DELTT=TIME( KN+1 ) -TIME( KN) 

STEPSZ=DEL‘rr/ FLOAT( ISTEP) 

DO 100 K=l, ISTEP 
TDOLD(LN) =TDSIM(LN) 

DO 40 LN=1, LINKS 

TDS IM( LN) =TDDS IM( LN) *STEPSZ+TDS IM( LN) 

TS IM( LN) =0 . 5 *STEPSZ* ( TDS IM( LN) +TDOLD( LN)) 

40 CONTINUE 

C SOLVE FINITE DIFFERENCE STEPS 

CALL KINEMT (TSIM.PAYLD) 

DO 50 1=1,15 
B(I)=0.0 
50 CONTINUE 
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CALL ARMl (TSIM,TDSIM,ZERO,FMK,B) 



C 

DO 80 1=1, LINKS 
DO 60 J=l, LINKS 
HJ(J)=0.0 
EJ(J)=0.0 
60 CONTINTJE 

EJ(I)=1.0 

CALL ARM2 (TSIM,EJ,HJ) 

DO 70 J=l, LINKS 
H(J,I)=HJ(J) 

70 CONTINUE 

80 CONTINUE 

DO 90 1=1, LINKS 
TDDSIM( I)=TAU( I)-B( I) 

90 CONTINUE 

IER=0 

C COMPUTE LINK ACCELERATIONS 

CALL TDDSOL (H,15,LINKS,LINKS,TDDSIM,15, 1,0, WORK, lER) 

100 CONTINUE 

C COMPUTE LINK ERROR DIFFERENCES 

DO 110 LN=1, LINKS 
TDDLST( LN) =TDDS IM( LN) 

TDLAST(LN) =TDSIM(LN) 

1LAST(LN)=TSIM(LN) 

POSSIM(LN,KN)=TSIM(LN) *R2D 
VELSIM(LN,KN)=TDSIM(LN) *R2D 
AaSIM(LN,KN)=TDDSIM(LN) *R2D 
ACCSIM(LN,KN)=TDDSIM(LN) *R2D 

S IMERR (LN) =SIMERR(LN) +ABS ( T2 (LN, KN) -ACCS IM(LN, KN) ) 

TOTACC ( LN) =T0TAC C ( LN ) +AB S ( T2 ( LN , KN ) ) 

110 CONTINUE 

C 

120 CONTINUE 
RETURN 
END 
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r *»»*»*»»»♦****»***»*****»******»♦»»****************** ***»***«**vi7T.sni. 
c***************************************************************wj.^n\. 
r»»***»***»» »»«**♦***«»♦**»*****»**»»*»»♦**♦*»**»»*»»* »****»»»«»VFT.Rni. 

SUBROUTINE VELSOL(X,y,Z,NDIM) 

REAL X(100),Y(100),Z(100) 

C 

SUM2=0. 

IF(NDIM-1)4,3,1 

C INTEGRATION LOOP 

1 DO 2 I=2,NDIM 
SUM1=SUJ!2 

SUM2=SUM2+. 5 * ( X( I) -X(I-1 ) ) * ( Y< I) +Y( I-l ) ) 

2 Z(I-1)=SUM1 

3 Z(NDIM)=SUM2 

4 RETURN 
END 



C********************************************4i*******************POSSOL 
C*********************************«**«**«************«**«**«*****POSSOL 
(;********************************************************* *******POSSOL 

SUBROUTINE POSSOL(X, Y,DERY,Z,NDIM) 

REAL X(IOO) , Y(IOO) ,DERY(100) ,Z(100) 

C 

SUM2=0. 

IF(NDIM-1)4,3,1 

C INTEGRATION LOOP 

1 DO 2 I=2,NDIM 
SUM1=SUM2 

SUM2=.5*(X(I)-X(I-D) 

SUM2=SUM1 +SUM2 *((Y(I)+Y(I-1))+.3333333*SUM2*( DERY < I-l ) -DERY ( I ) ) ) 

2 Z(I-1)=SUM1 

3 Z(NDIM)=SUM2 

4 RETURN 
END 
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*«*«*««*««*««««*««*«««««*««***««*«*«*«««««««««*««*«***«««*«* 

(« **«*«***«********«*«««««««««**«««««««««««««««««««***««**«**«*«*jp^gp 

C ***«**«**«**«**«»*«« 

REAL A(4,4,15) ,TT(4,4,15,2) ,T(15) ,TE(15,100,2) ,SMALLA(15) ,SMALLD(1 
15),ALPHA(15),TDDM 
INTEGER KNOTS 

C COMPUTE INDIVIDUAL LINK A-MATRICES 

SERR1=0.0 

SERR2=0.0 

SERR3=0.0 

PI=ARSIN(1.0)*2.0 

R2D=180.0/PI 

D2R=PI/ 180.0 

LINKS=6 

DO 21 1=1, LINKS 
SMALLA(I)=0.0 
SMALLD(I)=0.0 
ALPHA(I)=0.0 
21 CONTINUE 

SMALLA(2)=43.2 

SMALLA(3)=1.9 

SMALLD(3)=12.5 

SMALLD(4)=43.2 

ALPHA(1)=-90.0*D2R 

ALPHA(3)=90.0*D2R 

ALPHA(4)=-90.0*D2R 

ALPHA(5)=90.0*D2R 

C 

1=0 

7 1=1+1 

READ( 21 , END=1 ) TDUM, TE( 1 , I, 1 ) , TE(1 , I, 2) 

1 CONTINUE 

READ(22, *,END=2)TDUM,TE(2, 1,1) ,TE(2, I, 2) 

2 CONTINUE 

READ( 21 , *, END=3 ) TDUM,TE( 3 , I, 1 ) ,TE( 3 , I, 2) 

3 CONTINUE 

READ( 21 , •, END=4) TDUM,TE( 4, 1, 1 ) , TE( 4, 1, 2) 

4 CONTINUE 

READ( 21 , *, END=5 ) TDUM, TE( 5 , 1, 1 ) , TE( 5 , 1, 2) 

5 CONTINUE 

READ( 21 , * , END=6 ) TDUM, TE( 6, I , 1 ) , TE( 6 , I, 2) 

GO TO 7 

6 CONTINUE 
KNOTS=I-l 

DO 11 JJ=1, KNOTS 
DO 12 KK=1,2 
DO 10 1=1, LINKS 
T(I)=TE(I,JJ,KK) *D2R 
A(l,l,I)=COS(T(D) 
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A(l,2, I)=-COS(ALPHA( I) ) *SIN(T( I) ) 
A(1,3,I)=SIN(ALPHA(I))*SIN(T(I)) 

A(l, 4, I)=SMALLA( I) *COS(T( I) ) 

A(2,1,I)=SIN(T(I)) 

A( 2 , 2 , I) =COS ( ALPHA( I) ) *COS ( T( I) ) 
A(2,3,I)=-SIN(ALPHA(I))*C0S(T(D) 

A(2,4,I)=SMALLA(I)*SIN(T(D) 

A(3,1,I)=0.0 

A(3,2,I)=SIN(ALPHA(I)) 

A( 3 , 3 , I) =COS ( ALPHA( I) ) 

A(3,4,I)=SMALLD(I) 

A(4,1,I)=0.0 
A(4,2,I)=0.0 
A(4,3,I)=0.0 
A(4,4,I)=1.0 
10 CONTINUE 

C LOAD LINK TRANSPOSITION T-MATRICES 

DO 111 1=1, LINKS 
DO 112 J=l,4 
DO 113 K=l,4 
TT(J,K,I,KK)=0.0 
113 CONTINUE 
112 CONTINUE 
111 CONTINUE 

DO 110 1=1, LINKS 

DO 100 J=l,4 
DO 90 K=l,4 
TT(J,K,I,KK)=0.0 
DO 80 L=l,4 
IF (I.EQ.l) GO TO 60 

TT(J,K,I,KK)=TT(J,K,I,KK)+TT(J,L,I-1,KK) *A(L,K,I) 

GO TO 70 
60 CONTINUE 

TT(J,K,1,KK)=A(J,K,1) 

70 CONTINUE 

80 CONTINUE 

90 CONTINUE 

100 CONTINUE 

110 CONTINUE 

IF (KK.EQ.l)WRITE(61,200)Tr(l,4,6,l),Tr(2,4,6,l),Tr(3,4,6,l) 

IF (KK.EQ.2)WRITE(62,200)1T(1,4,6,2) ,TT(2,4,6,2) ,TT(3,4,6,2) 

12 CONTINUE 

ERRl=ABS(Tr(l,4,6,l)-Tr(l,4,6,2))/TT(l,4,6,l) 

ERR2=ABS(TT(2,4,6,1)-Tr(2,4,6,2))/TT(2,4,6,1) 

ERR3=ABS(TT(3,4,6,1)-Tr(3,4,6,2))/TT(3,4,6,1) 

WRITE( 63 , 200) ERRl , ERR2 , ERR3 

SERR1=ERR1+SERR1 

SERR2=ERR2+SERR2 
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SERR3=ERR3+SERR3 

11 CONTINUE 

ER1=SERR1/ FLOAT( KNOTS) 

ER2=SERR2/ FLOAT( KNOTS ) 

ER3 =SERR3 / FLOAT ( KNOTS ) 

WRITE( 63,230) ERl , ER2 , ER3 
RETURN 

200 FORMAT (3F15.8) 

230 FORMAT (/'TOTAL ERRORS =',3F15.8) 

END 



(]***************************************************** *******««*j^‘j<44 

********************************************* *«***********mAT44 

(;*******************«********************************* **********1^[^X44 

SUBROUTINE MAT44 (AA, I1,BB, Jl, CC, Kl) 

REAL AA(4,4,15),BB(4,4,15),CC(4,4,15) 

INTEGER I1,J1,K1 
DO 30 J=l,4 
DO 20 K=l,4 
CC(J,K,K1)=0.0 
DO 10 L=l,4 

CC(J,K,K1)=CC(J,K,K1)+AA(J,L,I1)*BB(L,K,J1) 

10 CONTINUE 

20 CONTINUE 

30 CONTINUE 

RETURN 
END 



C** ******************************************************* ******mAT44T 
C***************************************************************mat44T 

SUBROUTINE MAT44T (AA, I1,BB, Jl, CC,K1) 

REAL AA(4,4,15),BB(4,4,15),CC(4,4,15) 

INTEGER I1,J1,K1 
DO 30 J=l,4 
DO 20 K=l,4 
CC(J,K,K1)=0.0 
DO 10 L=l,4 

CC(J,K,K1)=CC(J,K,K1)+AA(J,L,I1)*BB(K,L,J1) 

10 CONTINUE 

20 CONTINUE 

30 CONTINUE 

RETURN 
END 
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(]*******************«*******************************************j^{;^‘j'04 
(^** *************************** **********************************] 4 ;^ 7(]4 
(]*«««**«*«**«***»« ***«**«**«**««*«***«**««««**«*****««**«**«**««j;{^‘j'04 

SUBROUTINE MATC4 ( Cl, AA, I1,BB, Jl) 

REAL AA(4,4,15),BB(4,4,15),C1 
INTEGER II, J1 
DO 20 1=1,4 
DO 10 J=l,4 

BB(I,J,J1)=C1*AA(I,J,I1) 

10 CONTINUE 

20 CONTINUE 

RETURN 
END 



C*** ********************************************************* 
d***** **************************************** ******************){;^1'Q 
(;****«*****««*«««*«**««*««*««**«**««*««*««**«**«**««*»«*«**«« 

SUBROUTINE MATCl ( AA, I1,BB, Jl, CC, Kl) 

REAL AA(15),BB(4,15),CC(4,15) 

INTEGER I1,J1,K1 
DO 10 1=1,4 

CC(I,K1)=AA( I1)*BB(I,J1) 

10 CONTINUE 
RETURN 
END 



Q**« ******************* 

(^*** *************************************** *********************){^J41 
C***** ****************************** ****************************]^'j'42 

SUBROUTINE MAT41 ( AA, I1,BB, Jl, CC, Kl) 

REAL AA(4,4,15),BB(4,15),CC(4,15) 

INTEGER I1,J1,K1 
DO 20 1=1,4 
CC(I,K1)=0.0 
DO 10 J=l,4 

CC(I,K1)=CC(I,K1)+AA(I,J,I1)*BB(J,J1) 

10 CONTINUE 

20 CONTINUE 

RETURN 
END 
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(^***«»*«**«*****««**«*4>«**«****«******4i****«******«**«*****«*«**]^{;^J24 
(^********4i**4i4i*4i***4i****4i4i******»**»******»*»»**»**»»*»**»**»***J^X14 
(;»**«**»**»**»»*»»*»»*»»**»»*»«*»»»»«*»«**««**«*«* *««*»«*»«*»*«»1^X14 

SUBROUTINE MAT14 ( AA, I1,BB, Jl, CC, Kl) 

REAL AA(4,15),BB(4,4,15),CC(4,15) 

INTEGER I1,J1,K1 
DO 20 1=1,4 
CC(I,K1)=0.0 
DO 10 J=l,4 

CC(I,K1)=CC(I,K1)+AA(J,I1)*BB(J,I,J1) 

10 CONTINUE 

20 CONTINUE 

RETURN 
END 



(^**«***«** ************************** ******* **4i***4> ****** ********j^D44 
^*********** ******************************************** ********^])44 
(]**** ***********************************************************^y)j)44 

SUBROUTINE ADD44 ( AA, I1,BB, Jl, CC,K1) 

REAL AA(4,4,15),BB(4,4,15),CC(4,4,15) 

INTEGER I1,J1,K1 
DO 20 1=1,4 
DO 10 1=1,4 

CC(I,J,J1)=AA(I,J,I1)+BB(I,J,K1) 

10 CONTINUE 

20 CONTINUE 

RETURN 
END 



Q***** ************************* *********************************^])H 
(;***************************************** **********************jy)])l]^ 
Q ************************** *************************** **********^£)21 

SUBROUTINE ADDll ( AA, I1,BB, Jl, CC, Kl) 

REAL AA(4,15),BB(4,15),CC(4,15) 

INTEGER I1,J1,K1 
DO 10 1=1,4 

CC(I,K1)=AA(I,I1)+BB(I,J1) 

10 CONTINUE 
RETURN 
END 
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Q************************************* **************** ***«*****«j^{)y^L 

(]**»«**************«**♦*«**«*««****««********** *««*«************;^D;\Ll 

(3*** ***««****«****«****«**********«*«**««*«*«* *«****«******»****jy)])y^{^ 

SUBROUTINE ABDALL (AA, I1,BB,J1,CC,K1,DD,L1,EE,M1) 

REAL AA(4,4,15),BB(4,4,15),CC(4,4,15),DD(4,4,15),EE(4,4,15) 
INTEGER I1,J1,K1,L1,M1 
DO 20 1=1,4 
DO 10 J=l,4 

EE(I,J,M1)=AA(I,J,I1)+BB(I,J,J1)+CC(I,J,K1)+DD(I,J,L1) 

10 CONTINUE 

20 CONTINUE 

RETURN 
END 



(;*««****»**»**************«******************************«**«**«pj{{3<f 
Q««***********«***«**«*«««*«******«**«*********«***«*««*«**«**««pjp3'p 
******************************************************* ***pjp3'p 

SUBROUTINE FIRST (PTQ, I1,DD, C17) 

REAL PTQ(4,4,15),DD(4,4,15),CCl(4,4.15),a7 
INTEGER II 

CALL MAT44 (PTO, II, DD, II , CCl, II) 

Cl 7=0.0 

DO 10 1=1,4 

a7=C17+Ca(I,I,Il) 

10 CONTINUE 
RETURN 
END 

(]************«*«***«**««*««**«**««**« *****«**«**««**«*««*****«**3£QQ[^ 

C**« ********** **************************************************SECOND 
C**« ********************************************************* ***SECOND 

SUBROUTINE SECOND (GRAV,PTQ, II , CC, Q9) 

REAL GRAV(4,15),PTO(4,4,15),CC(4,15),CC2(4,15),C19 
INTEGER II 

CALL MAT14 (GRAY, II, PTQ, II, CC2, II) 

a9=o.o 

DO 10 J=l,4 

C19=a9+CC2(J, ID *00(1,11) 

10 CONTINUE 
RETURN 
END 
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